diff --git a/Clean_Time_domain_BP.py b/Clean_Time_domain_BP.py new file mode 100644 index 0000000..31bf9f0 --- /dev/null +++ b/Clean_Time_domain_BP.py @@ -0,0 +1,731 @@ +""" +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 numpy as np +import matplotlib.pyplot as plt +from scipy.ndimage import gaussian_filter, label +from pathlib import Path + +# ══════════════════════════════════════════════════════ +# 0.1 ИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ +# ══════════════════════════════════════════════════════ +INPUT_IDX = [0, 1, 2, 3] +OUTPUT_IDX = [2, 3] + +# Частотный диапазон и глубинный gate оставлены в прошлой логике. +F_START = 27 * 1e8 +F_STOP = 6*1e9 +MIN_DEPTH = 2.7 +MAX_DEPTH = 15.0 + +# Данные и Вычитание среднего фона - как обычно +BG_SUBTRACT = True +BG_PATH = Path('/Users/ivan_root/Desktop/GPR data/20260422_for_Vanya/20260422_night/20260422_evening_scene_balvanka_z/preprocessed') + +DATA_PATH = Path('/Users/ivan_root/Desktop/GPR data/20260422_for_Vanya/20260422_night/20260422_evening_scene_balvanka_z/preprocessed/0002_id1_ns5952943855654') + +# Убрать паразитные боковые лепестки с 2D карты +BP_REMOVE_SIDELOBE_OBJECTS = True # True: убрать SL-кандидаты из финальной таблицы и разметки + +# Компенсация затухания в этой версии разделена на 2 части: +# range: геометрическое расхождение 1/(Rtx*Rrx) +# angle: диаграмма направленности cos_tx^2 * cos_rx^2 +COMP_RANGE_POWER = 0.28 # Можно менять свободно, текущее значение подобрано экспериментально +COMP_ANGLE_POWER = 0.10 # Можно менять но лучше ставить не больше 0.25. Текущее значение подобрано нормально + +# ══════════════════════════════════════════════════════ +# 0.2 КОНФИГИ АНТЕНН +# ══════════════════════════════════════════════════════ + +# Физические координаты антенн по их реальным индексам, [м] - текущий конфиг +TX_POSITIONS = { + 2: -74.5 * 0.01, + 3: 75.0 * 0.01, +} + +RX_POSITIONS = { + 0: 19.0 * 0.01, + 1: 44.5 * 0.01, + 2: -40.0 * 0.01, + 3: -19.0 * 0.01, +} +# Вариант для теста на старых конфигурациях +# TX_POSITIONS = {0: 90.5*0.01, # Tx с индексом 0 → x = +0.9 м +# 3: -90.5*0.01} # Tx с индексом 3 → x = -0.9 м + +# RX_POSITIONS = {0: -18*0.01, +# 1: 48.5*0.01, +# 2: -49.0*0.01, +# 3: 18.5*0.01} + +# ══════════════════════════════════════════════════════ +# 0.3 НЕИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ (ЛУЧШЕ НЕ ТРОГАТЬ) +# ══════════════════════════════════════════════════════ + +eps_r = 1.0 +v = 3e8 / np.sqrt(eps_r) + +x_tx = np.array([TX_POSITIONS[i] for i in sorted(TX_POSITIONS)]) +x_rx = np.array([RX_POSITIONS[j] for j in sorted(RX_POSITIONS)]) + + +# Сетка BP-карты +x_min, x_max = x_tx.min() - 2.0, x_tx.max() + 2.0 +z_min, z_max = 0.1, MAX_DEPTH +NX_BP = 300 +NZ_BP = 300 + +# Oversampling A-сканов: искусственно повышает плотность точек по t, но не физическое разрешение. +BP_OVERSAMPLE = 8 + +# Компенсация ослабления разделена на две части: +# range: геометрическое расхождение 1/(Rtx*Rrx) +# angle: диаграмма направленности cos_tx^2 * cos_rx^2 +# Компенсация нормируется на точку под виртуальным центром пары на глубине COMP_REF_DEPTH. +COMP_RANGE_WEIGHT_MAX = 5.0 +COMP_ANGLE_WEIGHT_MAX = 2.0 +COMP_WEIGHT_MAX = 8.0 # общий финальный потолок после перемножения range*angle +COMP_REF_DEPTH = 3.0 + +# Сглаживание только для удобства поиска/визуализации максимума. +BP_SMOOTH_SIGMA = 1.5 + +# Параметры поиска объектов на BP-карте. +MAX_OBJECTS = 10 +BP_OBJECT_MIN_FRAC = 0.35 # остановка: пик ниже этой доли от глобального максимума +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 # кандидат должен быть слабее родительского максимума + + + + +# ══════════════════════════════════════════════════════ +# 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.' + +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}') + + +# ══════════════════════════════════════════════════════ +# 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 = {} + +for (i, j), s21 in s21_data.items(): + s21_proc = s21.copy() + if BG_SUBTRACT and background is not None and (i, j) in background: + s21_proc = s21_proc - background[(i, j)] + + t_pair, a_pair, h_pair, n_fft_base, n_fft = compute_ascan_bp( + s21_proc, + freq_data[(i, j)], + f_start=F_START, + f_stop=F_STOP, + 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) + +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} см') + + +# ══════════════════════════════════════════════════════ +# 3. TIME-DOMAIN INCOHERENT 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 attenuation_components_map(i_tx, i_rx, XX, ZZ): + Rtx = np.sqrt((XX - x_tx[i_tx])**2 + ZZ**2) + Rrx = np.sqrt((XX - x_rx[i_rx])**2 + ZZ**2) + geo = 1.0 / (Rtx * Rrx + 1e-12) + angle = (ZZ / (Rtx + 1e-12))**2 * (ZZ / (Rrx + 1e-12))**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 = np.sqrt((xc - x_tx[i_tx])**2 + z_ref**2) + Rrx = np.sqrt((xc - x_rx[i_rx])**2 + z_ref**2) + geo = 1.0 / (Rtx * Rrx + 1e-12) + angle = (z_ref / (Rtx + 1e-12))**2 * (z_ref / (Rrx + 1e-12))**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 = np.sqrt((XX_bp - x_tx[i])**2 + ZZ_bp**2) + Rrx = np.sqrt((XX_bp - x_rx[j])**2 + ZZ_bp**2) + tau = (Rtx + Rrx) / v + 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 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, + '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 bistatic_depth_signature(x_obj, z_obj): + signature = [] + for i in range(N_tx): + for j in range(N_rx): + Rtx = np.sqrt((x_obj - x_tx[i])**2 + z_obj**2) + Rrx = np.sqrt((x_obj - x_rx[j])**2 + z_obj**2) + 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 + + +#print('Расчет coherent time-domain BP...', end=' ', flush=True) +bp_raw, bp_complex = backproject_coherent(H_bp, T_bp, compensate=True) +bp_map = bp_raw / (bp_raw.max() + 1e-12) +bp_map_s = gaussian_filter(bp_map, sigma=BP_SMOOTH_SIGMA) +bp_map_s = np.where(depth_gate, bp_map_s, 0.0) +bp_map_s = bp_map_s / (bp_map_s.max() + 1e-12) +#print('готово.') + + +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 = mark_sidelobe_candidates(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 = [obj for obj in bp_objects_all if not obj['sidelobe_candidate']] +else: + bp_objects = bp_objects_all + +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 + +# print('\n' + '=' * 72) +# print(' COHERENT TIME-DOMAIN BP: максимум карты и центроид области') +# print('=' * 72) +# print(f' argmax: x = {x_peak*100:+.1f} см, z = {z_peak*100:.1f} см, BP = {peak_value:.3f}') +# if main_obj is not None: +# print(f" centroid: x = {main_obj['x']*100:+.1f} см, z = {main_obj['z']*100:.1f} см, " +# f"area = {main_obj['area_cm2']:.1f} см^2") +# print('=' * 72) + +# print('\n' + '=' * 72) +# n_sl_all = sum(obj['sidelobe_candidate'] for obj in bp_objects_all) +# if BP_REMOVE_SIDELOBE_OBJECTS: +# print(f' НАЙДЕННЫЕ ОБЪЕКТЫ НА COHERENT BP-КАРТЕ, max {MAX_OBJECTS} ' +# f'(SL скрыты: {n_sl_all})') +# else: +# print(f' НАЙДЕННЫЕ ОБЪЕКТЫ НА COHERENT BP-КАРТЕ, max {MAX_OBJECTS} ' +# f'(SL показаны: {n_sl_all})') +# print('=' * 126) +# print(f" {'#':<4} {'Xc [см]':>10} {'Zc [см]':>10} {'Xmax [см]':>11} {'Zmax [см]':>11} " +# f"{'Peak':>8} {'Area [см2]':>11} {'PhCoh':>7} {'PhVar':>7} {'SL?':>5} {'Parent':>6} {'RMSr [см]':>10}") +# print('-' * 126) +# for obj in bp_objects: +# sl_label = 'yes' if obj['sidelobe_candidate'] else 'no' +# parent_label = '-' if obj['sidelobe_parent'] is None else str(obj['sidelobe_parent']) +# rms_label = '-' if np.isnan(obj['sidelobe_range_rms_cm']) else f"{obj['sidelobe_range_rms_cm']:.1f}" +# print(f" {obj['index']:<4} {obj['x']*100:>+10.1f} {obj['z']*100:>10.1f} " +# f"{obj['x_peak']*100:>+11.1f} {obj['z_peak']*100:>11.1f} " +# f"{obj['peak']:>8.3f} {obj['area_cm2']:>11.1f} " +# #f"{obj['phase_coherence']:>7.3f} {obj['phase_circular_variance']:>7.3f} " +# f"{sl_label:>5} {parent_label:>6} {rms_label:>10}") +# print('=' * 126) + + +# ══════════════════════════════════════════════════════ +# 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, # Было 0.0 + vmax=0.95, # Было 1.0 +) +plt.colorbar(im, ax=ax, label='Нормированная |coherent BP|') +ax.plot(x_tx * 100, np.zeros(N_tx), 'r^', ms=12, label='Tx', zorder=5) +ax.plot(x_rx * 100, np.zeros(N_rx), 'bv', ms=12, label='Rx', zorder=5) +for obj in bp_objects: + 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['index']) + text_color = 'white' + ax.text(obj['x'] * 100 + 3, obj['z'] * 100, label_text, + color=text_color, fontsize=9, weight='bold', zorder=7) + +ax.plot(x_peak * 100, z_peak * 100, 'w*', ms=18, markeredgecolor='k', mew=0.9, + label='Глобальный максимум BP', zorder=6) +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 = 'Time-domain coherent BackProjection с geo·pattern компенсацией' +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() \ No newline at end of file diff --git a/GPR_motion_algo.py b/GPR_motion_algo.py deleted file mode 100644 index 58a3e39..0000000 --- a/GPR_motion_algo.py +++ /dev/null @@ -1,787 +0,0 @@ -""" -MIMO GPR — локализация через пересечение эллипсов -================================================== - -Физика в двух словах: - Пик A-скана пары (Tx_i, Rx_j) на задержке τ означает: - |Tx → объект| + |объект → Rx| = v · τ - Это уравнение эллипса. Истинный отражатель лежит на - пересечении всех 16 эллипсов (по одному на пару). - -Алгоритм: - 1. S(f) → IFFT → 16 A-сканов - 2. Поиск пиков: SNR = пик / медиана > порог - 3. Для каждого пика → мягкий эллипс в аккумуляторе - (с компенсацией геометрического и углового затухания) - 4. CLEAN: найти максимум → убрать его эллипсы → повторить - -О параметре SHELL_SIGMA: - Аккумулятор — это «мягкое голосование». Каждый эллипс добавляет - не единицу, а гауссово-взвешенный вклад: - w = exp(−δ²/2σ²), где δ = |R_Tx + R_Rx − v·τ| - SHELL_SIGMA — ширина этой гауссовой оболочки. - Слишком широко → ghost-цели не подавляются. - Слишком узко → вклад падает до нуля из-за дискретности сетки. - Оптимум: ~ 0.4 × δZ, где δZ = v/(2B) — разрешение по глубине. - -О score: - score = количество пар (из 16), чей эллипс проходит - через данную точку с невязкой δ < 3σ. - Принимает целые значения от 0 до 16. - Максимальный score у истинного объекта = 16 (все пары согласны). - Ghost-цели имеют меньший score, т.к. согласуются только - с частью пар. - -О компенсации затухания: - При генерации S(f) сигнал ослаблен: - geo(i,j) = 1/(R_Tx · R_Rx) — геометрическое ослабление - pat(i,j) = cos²(θ_Tx)·cos²(θ_Rx) — диаграмма направленности - Без компенсации глубокий/угловой отражатель будет недооценён. - Компенсация: делим вес каждого пика на ожидаемое затухание - в точке z_apparent, вычисленное для данной пары антенн. - -Геометрия: плоскость XZ (X — вдоль антенн, Z — глубина). -""" - -""" -MIMO GPR — локализация через пересечение эллипсов -================================================== -Версия для реальных данных -""" - -import numpy as np -import matplotlib.pyplot as plt -from matplotlib.lines import Line2D -from scipy.signal import find_peaks -from scipy.ndimage import gaussian_filter, label -from pathlib import Path -from dataclasses import dataclass, field -from typing import Dict, List, Tuple - -### Изменяемые параметры ====== -INPUT_IDX = [0,1,2,3] -OUTPUT_IDX = [0,3] - -MIN_DEPTH = 3.0 # [м] пропустить прямую волну -MAX_DEPTH = 20.0 -COMP_POWER = 0.22 # степень компенсации затухания - -# Обрезка по частоте -F_START = 28*1e8 # Нижняя частота -F_STOP = 60*1e8 # Верхняя частота - -# Параметры скорости в Motion Config -SPEED_M_S = 0.48 # Скорость м/с -LOOK_ANGLE_DEG = 2.0 # Угол наклона радара отн-но горизонтали (град) - -# Вычитание среднего фона (background removal) -# True → вычитать среднее по всем снимкам в папке (убирает прямую волну и статичные отражения) -# False → использовать данные как есть -BG_SUBTRACT = True -BG_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed') - -DATA_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed/0002_id3_ns26032413993535') # <-- УКАЖИТЕ ПУТЬ - -# =============================== - -@dataclass -class MotionConfig: - """ - Конфигурация движения для одного кадра из 8 пар. - - speed_m_s: - Линейная скорость движения радара. - - look_angle_deg: - Угол между направлением движения и осью дальности Z. - Если движение почти "вдоль дальности", ставьте угол близкий к 0°. - Тогда dz = v_move * dt * cos(angle) ≈ v_move * dt. - - sweep_time_s: - Время прохода по всем частотам для одной пары. - - switch_time_s: - Время переключения между соседними парами. - - pair_order_phys: - Реальный порядок измерения в физических индексах: - [(tx_phys_1, rx_phys_1), (tx_phys_2, rx_phys_2), ...] - - reference_mode: - Относительно какого момента считаем dt: - - 'frame_center' : середина всего цикла по 8 парам - - 'first_pair' : центр первой пары - - direction_sign: - Знак движения по оси дальности. - +1 -> более поздние пары выглядят глубже - -1 -> более поздние пары выглядят ближе - """ - speed_m_s: float = 0.50 - look_angle_deg: float = 0.0 - sweep_time_s: float = 0.040 - switch_time_s: float = 0.005 - pair_order_phys: List[Tuple[int, int]] = field(default_factory=lambda: [ - (tx_phys, rx_phys) - for tx_phys in sorted(OUTPUT_IDX) - for rx_phys in sorted(INPUT_IDX) - ]) - reference_mode: str = 'frame_center' - direction_sign: float = +1.0 - -# Конфигурация движения -MOTION_CONFIG = MotionConfig( - speed_m_s=SPEED_M_S, - look_angle_deg=LOOK_ANGLE_DEG, - sweep_time_s=0.15, # ref 0.15 - switch_time_s=1e-5, - pair_order_phys=[ - (0, 0), (0, 1), (0, 2), (0, 3), # Этот порядок текущий, возможно в будущем что-то поменяется - (3, 0), (3, 1), (3, 2), (3, 3), - ], - reference_mode='frame_center', - direction_sign=+1.0, -) - -### Список параметров и констант использующиеся в коде: ### - - -MODE = 'point' # Один из 2х режимов `point` or `extended` - -eps_r = 1.0 # Диэлектрическая проницаемость среды -v = 3e8 / np.sqrt(eps_r) # скорость света в среде - -# ══════════════════════════════════════════════════════ -# КООРДИНАТЫ АНТЕНН — УКАЖИТЕ РЕАЛЬНЫЕ ЗНАЧЕНИЯ! -# ══════════════════════════════════════════════════════ -# Координаты вдоль оси X на поверхности (z = 0), в метрах - -# Физические координаты антенн по их реальным индексам -TX_POSITIONS = {0: 90.5*0.01, # Tx с индексом 0 → x = +0.9 м - 3: -90.5*0.01} # Tx с индексом 3 → x = -0.9 м - -RX_POSITIONS = {0: -18*0.01, - 1: 48.5*0.01, - 2: -49.0*0.01, - 3: 18.5*0.01} - -# Массивы позиций в порядке возрастания физических индексов -# (нужны для сетки аккумулятора и графиков) -x_tx = np.array([TX_POSITIONS[i] for i in sorted(TX_POSITIONS)]) -x_rx = np.array([RX_POSITIONS[j] for j in sorted(RX_POSITIONS)]) - -# Параметры алгоритма -SNR_THRESH = 4.5 # минимальный SNR пика -SNR_COMP_MAX = 25.0 # Верхний порог для компенсированного значения SNR - - -# Параметр: максимальное число объектов для поиска -MAX_OBJECTS = 15 # <-- настройте под вашу задачу - -# Границы сетки аккумулятора -x_min, x_max = x_tx.min() - 2.0, x_tx.max() + 2.0 # [м] -z_min, z_max = 0.2, MAX_DEPTH # <-- глубина [м] - - - -# ══════════════════════════════════════════════════════ -# 1. ЗАГРУЗКА РЕАЛЬНЫХ ДАННЫХ -# ══════════════════════════════════════════════════════ - -def load_mimo_data(data_path, input_idx, output_idx): - data_path = Path(data_path) - s21_data = {} - freq_data = {} - - for f in data_path.glob("i*_o*_s21.npy"): - name = f.stem - parts = name.split('_') - i_tx_phys = int(parts[1][1:]) - i_rx_phys = int(parts[0][1:]) - - if i_tx_phys not in output_idx or i_rx_phys not in input_idx: - continue - - # Переводим физический индекс → порядковый (0,1,2,...) - i_tx = sorted(output_idx).index(i_tx_phys) - i_rx = sorted(input_idx).index(i_rx_phys) - - s21_data[(i_tx, i_rx)] = np.load(f) - freq_file = data_path / f"i{i_rx_phys}_o{i_tx_phys}_freq.npy" - freq_data[(i_tx, i_rx)] = np.load(freq_file) - - tx_indices = sorted(set(k[0] for k in s21_data)) - rx_indices = sorted(set(k[1] for k in s21_data)) - n_tx = len(tx_indices) - n_rx = len(rx_indices) - - print(f"Загружено пар: {len(s21_data)}") - print(f"Передатчиков: {n_tx}, Приёмников: {n_rx}") - - return s21_data, freq_data, n_tx, n_rx - - -# Загрузка данных -s21_data, freq_data, N_tx, N_rx = load_mimo_data(DATA_PATH, INPUT_IDX, OUTPUT_IDX) -N_pairs = len(s21_data) - -# ══════════════════════════════════════════════════════ -# 1б. ВЫЧИСЛЕНИЕ СРЕДНЕГО ФОНА ПО ВСЕМ СНИМКАМ -# ══════════════════════════════════════════════════════ - -def compute_background(bg_path, input_idx, output_idx): - """ - Для каждой пары (i_tx, i_rx) усредняем S21 по всем снимкам в папке. - - Возвращает: - bg : dict[(i_tx, i_rx)] → np.array (complex), усреднённый S21 - """ - bg_path = Path(bg_path) - snapshots = sorted(bg_path.glob("*/")) # каждый подкаталог — один снимок - snapshots = [s for s in snapshots if s.is_dir()] - - if len(snapshots) == 0: - print("⚠️ Снимков для фона не найдено, BG_SUBTRACT отключён.") - return None - - print(f"Вычисление фона по {len(snapshots)} снимкам...", end=" ", flush=True) - - # Накопитель: для каждой пары суммируем S21 - bg_sum = {} - bg_count = {} - - for snap_dir in snapshots: - for f in snap_dir.glob("i*_o*_s21.npy"): - name = f.stem - parts = name.split('_') - i_tx_phys = int(parts[1][1:]) - i_rx_phys = int(parts[0][1:]) - - if i_tx_phys not in output_idx or i_rx_phys not in input_idx: - continue - - i_tx = sorted(output_idx).index(i_tx_phys) - i_rx = sorted(input_idx).index(i_rx_phys) - key = (i_tx, i_rx) - - s21 = np.load(f) - if key not in bg_sum: - bg_sum[key] = np.zeros_like(s21, dtype=complex) - bg_count[key] = 0 - bg_sum[key] += s21 - bg_count[key] += 1 - - bg = {key: bg_sum[key] / bg_count[key] for key in bg_sum} - print(f"готово. Пар: {len(bg)}, снимков на пару: " - f"{list(bg_count.values())[0] if bg_count else 0}") - return bg - - -if BG_SUBTRACT: - background = compute_background(BG_PATH, INPUT_IDX, OUTPUT_IDX) - if background is None: - BG_SUBTRACT = False # автоматически выключаем если нет данных -else: - background = None - print("BG_SUBTRACT = False, вычитание фона отключено.") - - -# Проверка частот (берём из первой пары как референс) -first_key = list(freq_data.keys())[0] -freqs = freq_data[first_key] - - - -# Проверим что частоты одинаковые для всех пар -for key, freq in freq_data.items(): - if not np.allclose(freq, freqs): - print(f"⚠️ Частоты для пары {key} отличаются!") - - -mask_freq = (freqs >= F_START) & (freqs <= F_STOP) - -freqs = freqs[mask_freq] - -f_min, f_max = freqs[0], freqs[-1] -BW = f_max - f_min -N_f = len(freqs) - - -# ══════════════════════════════════════════════════════ -# 2. ПАРАМЕТРЫ СИСТЕМЫ -# ══════════════════════════════════════════════════════ - -# Проверка соответствия координатов антенн -assert len(x_tx) == N_tx, f"x_tx должен содержать {N_tx} элементов" -assert len(x_rx) == N_rx, f"x_rx должен содержать {N_rx} элементов" - - -# SHELL_SIGMA — ширина гауссовой оболочки -SHELL_SIGMA = v / BW * 0.5 # [м] - - -# Сетка аккумулятора - -x_grid = np.linspace(x_min, x_max, 300) -z_grid = np.linspace(z_min, z_max, 300) -XX, ZZ = np.meshgrid(x_grid, z_grid) - -# Расстояния от сетки до каждой антенны -R_tx_grid = {i: np.sqrt((XX - x_tx[i])**2 + ZZ**2) for i in range(N_tx)} -R_rx_grid = {j: np.sqrt((XX - x_rx[j])**2 + ZZ**2) for j in range(N_rx)} - - -# ══════════════════════════════════════════════════════ -# 3. ВЫЧИСЛЕНИЕ A-СКАНОВ ИЗ РЕАЛЬНЫХ S21 -# ══════════════════════════════════════════════════════ - -def compute_ascan(s21, freq, f_start, f_stop, window=True): - """ - S21(f) → IFFT → A-скан с правильным частотным сдвигом. - - Проблема наивного подхода (buf[:n] = s21): - IFFT считает, что спектр начинается с 0 Гц. - Реальные данные начинаются с f[0] > 0, поэтому - нулевая задержка смещается и в A-скане появляются биения. - - Правильный подход — сдвиг спектра: - Шаг частотной сетки df вычисляется из данных. - Индекс первой частоты: k0 = round(f[0] / df). - Данные кладутся в H[k0 : k0+n], а не в H[0 : n]. - Тогда IFFT корректно восстанавливает временной сигнал - с нулевой задержкой в t=0. - - Размер FFT: - Минимум для покрытия всего диапазона [0, f[-1]]: - min_len = 2 * (k0 + n - 1) - Округляем вверх до степени двойки для скорости FFT. - """ - mask_freq_ = (freq >= f_start) & (freq <= f_stop) - freq = freq[mask_freq_] - s21 = s21[mask_freq_] - - n = len(freq) - if n < 2: - raise ValueError("Слишком мало частотных точек") - - # Шаг частотной сетки - df = (freq[-1] - freq[0]) / (n - 1) - if df <= 0: - raise ValueError("Частоты не возрастают") - - # Индекс первой частоты в полной сетке от 0 до f[-1] - k0 = int(np.round(freq[0] / df)) - - # Минимальный размер FFT, округлённый до степени двойки - min_len = 2 * (k0 + n - 1) - n_fft = 1 << int(np.ceil(np.log2(min_len))) - - - # Временна́я ось — пересчитываем из нового n_fft - dt = 1.0 / (n_fft * df) - t_sec = np.arange(n_fft, dtype=float) * dt - - # Оконная функция (подавление боковых лепестков IFFT) - s = s21 * np.hanning(n) if window else s21.copy() - - # Спектр со сдвигом: данные на своём месте в частотной сетке - H = np.zeros(n_fft, dtype=np.complex128) - H[k0 : k0 + n] = s - - y = np.abs(np.fft.ifft(H)) - - - return t_sec[:y.size], y[:y.size] - - -print("Вычисление A-сканов из реальных данных...", end=" ", flush=True) - -A = {} # A[(i,j)] — амплитудный A-скан -T_h = {} # T_h[(i,j)] — временна́я ось для этой пары [с] -Z_h = {} # Z_h[(i,j)] — ось глубины [м] - -for (i, j), s21 in s21_data.items(): - s21_proc = s21.copy() - - # Вычитание фона в частотной области - if BG_SUBTRACT and background is not None and (i, j) in background: - s21_proc = s21_proc - background[(i, j)] - # Примечание: вычитаем до обрезки по частоте и до окна — - # фон вычисляется из полных (необрезанных) данных, - # поэтому вычитание корректно в полном частотном диапазоне. - - t_pair, a_pair = compute_ascan(s21_proc, freq_data[(i, j)], - f_start=F_START, f_stop=F_STOP) - T_h[(i, j)] = t_pair - Z_h[(i, j)] = t_pair * v / 2 - A[(i, j)] = a_pair - -bg_label = "с вычитанием фона" if BG_SUBTRACT else "без вычитания фона" -print(f"готово ({bg_label}).") - - -# Общая ось z для визуализации и поиска пиков -# (берём максимальный диапазон по всем парам) -z_h = Z_h[list(Z_h.keys())[0]] # все пары дают одинаковую ось, если freq совпадают -t_h = T_h[list(T_h.keys())[0]] - - -# ══════════════════════════════════════════════════════ -# 4. ДЕТЕКТИРОВАНИЕ ПИКОВ -# ══════════════════════════════════════════════════════ - -def attenuation_at_depth(i_tx, i_rx, z_app): - """ - Ожидаемое ослабление geo·pattern для точки прямо под виртуальным - центром пары на глубине z_app. - - Используется для компенсации: реальный SNR пика делится на это - значение, чтобы вес глубокого/углового объекта не занижался. - """ - xc = (x_tx[i_tx] + x_rx[i_rx]) / 2.0 # виртуальный центр - Rtx = np.sqrt((xc - x_tx[i_tx])**2 + z_app**2) - Rrx = np.sqrt((xc - x_rx[i_rx])**2 + z_app**2) - geo = 1.0 / (Rtx * Rrx + 1e-12) - pat = (z_app / (Rtx + 1e-12))**2 * (z_app / (Rrx + 1e-12))**2 - return geo * pat + 1e-30 # +ε чтобы не делить на ноль - - -def find_peaks_snr(i_tx, i_rx, SNR_COMP_MAX = SNR_COMP_MAX): - """ - Поиск пиков A-скана. - Возвращает список dict: - z_app — кажущаяся глубина [м] - tau — задержка [с] - snr_raw — SNR без компенсации = пик / медиана - snr_comp— SNR с компенсацией ослабления (используется в аккумуляторе) - """ - ascan = A[(i_tx, i_rx)] - z_h_ij = Z_h[(i_tx, i_rx)] - t_h_ij = T_h[(i_tx, i_rx)] - i_min = np.searchsorted(z_h_ij, MIN_DEPTH) - i_max = np.searchsorted(z_h_ij, MAX_DEPTH) - - noise = np.median(ascan[i_min:i_max]) # Добавил чтобы было удобно считать SNR отнсительно выбранной области - min_dist = max(4, int(v / (2*BW) / (z_h_ij[1] - z_h_ij[0]) * 0.7)) - idx, _ = find_peaks(ascan[i_min:i_max], - height=noise * SNR_THRESH, - distance=min_dist) - idx += i_min - result = [] - for p in idx: - z_app = float(z_h_ij[p]) - snr_raw = float(ascan[p] / noise) - atten = attenuation_at_depth(i_tx, i_rx, z_app) - atten_norm = atten / attenuation_at_depth(i_tx, i_rx, 3.0) # Референсная глубина - 3м - snr_comp = snr_raw / (atten_norm ** COMP_POWER + 1e-12) - snr_comp = min(snr_comp, SNR_COMP_MAX) # ← clipping - result.append({'z_app': z_app, - 'tau': float(t_h_ij[p]), - 'snr_raw': snr_raw, - 'snr_comp': snr_comp}) - return result - - -peaks = {(i, j): find_peaks_snr(i, j) - for i in range(N_tx) for j in range(N_rx)} - -# ══════════════════════════════════════════════════════ -# 6. ПОИСК ОБЪЕКТОВ -# ══════════════════════════════════════════════════════ - -def find_centroid(acc_s, iz, ix, rpz, rpx): - """ - Взвешенный центроид аккумулятора в окрестности (iz, ix). - """ - NZ, NX = acc_s.shape - iz0 = max(0, iz - rpz); iz1 = min(NZ, iz + rpz) - ix0 = max(0, ix - rpx); ix1 = min(NX, ix + rpx) - patch = acc_s[iz0:iz1, ix0:ix1].copy() - W = patch.sum() - if W <= 0: - return x_grid[ix], z_grid[iz] - rows = np.arange(iz0, iz1)[:, None] * np.ones(patch.shape) - cols = np.ones(patch.shape) * np.arange(ix0, ix1)[None, :] - iz_c = int(round(np.clip((rows * patch).sum() / W, 0, NZ-1))) - ix_c = int(round(np.clip((cols * patch).sum() / W, 0, NX-1))) - return x_grid[ix_c], z_grid[iz_c] - - -# ══════════════════════════════════════════════════════ -# 8. MOTION-AWARE FIRST-ORDER CORRECTION -# ══════════════════════════════════════════════════════ - -""" -Первый блок для движения без изменения продакшн-пайплайна выше. - -Идея: -1. Для каждой пары (Tx, Rx) задаём время центра её измерения. -2. По известной скорости и углу получаем сдвиг по дальности dz. -3. Переводим dz в поправку по задержке dtau. -4. Для уже найденных пиков формируем motion-corrected версию: - tau_corr, z_corr - -Это first-order модель: считаем, что вся пара измерена в момент времени t_center. -Если позже окажется, что смещение за один sweep пары уже заметно, -следующим шагом надо будет делать per-frequency коррекцию до IFFT. -""" - - -def _build_phys_to_logical_maps(output_idx, input_idx): - tx_map = {phys: k for k, phys in enumerate(sorted(output_idx))} - rx_map = {phys: k for k, phys in enumerate(sorted(input_idx))} - return tx_map, rx_map - - - -def compute_pair_timestamps(config: MotionConfig, - output_idx=OUTPUT_IDX, - input_idx=INPUT_IDX) -> Tuple[Dict[Tuple[int, int], Dict], List[Dict]]: - """ - Для каждой пары возвращает: - t_start, t_center, dt_ref, dz_motion, dtau_motion - - Ключи словаря pair_timestamps — логические индексы (i_tx, i_rx), - совместимые с существующим словарём peaks. - """ - tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx) - - rows: List[Dict] = [] - t_cursor = 0.0 - - for order_idx, (tx_phys, rx_phys) in enumerate(config.pair_order_phys): - if tx_phys not in tx_phys_to_log: - raise ValueError(f"Tx {tx_phys} отсутствует в OUTPUT_IDX={output_idx}") - if rx_phys not in rx_phys_to_log: - raise ValueError(f"Rx {rx_phys} отсутствует в INPUT_IDX={input_idx}") - - i_tx = tx_phys_to_log[tx_phys] - i_rx = rx_phys_to_log[rx_phys] - - t_start = t_cursor - t_center = t_start + 0.5 * config.sweep_time_s - t_stop = t_start + config.sweep_time_s - - rows.append({ - 'order_idx': order_idx, - 'tx_phys': tx_phys, - 'rx_phys': rx_phys, - 'i_tx': i_tx, - 'i_rx': i_rx, - 't_start_s': t_start, - 't_center_s': t_center, - 't_stop_s': t_stop, - }) - - t_cursor = t_stop + config.switch_time_s - - if not rows: - return {}, [] - - if config.reference_mode == 'frame_center': - t_ref = 0.5 * (rows[0]['t_center_s'] + rows[-1]['t_center_s']) - elif config.reference_mode == 'first_pair': - t_ref = rows[0]['t_center_s'] - else: - raise ValueError("reference_mode must be 'frame_center' or 'first_pair'") - - cos_theta = np.cos(np.radians(config.look_angle_deg)) - pair_timestamps: Dict[Tuple[int, int], Dict] = {} - - for row in rows: - dt_ref = row['t_center_s'] - t_ref - dz_motion = config.direction_sign * config.speed_m_s * dt_ref * cos_theta - dtau_motion = 2.0 * dz_motion / v - - row['dt_ref_s'] = dt_ref - row['dz_motion_m'] = dz_motion - row['dtau_motion_s'] = dtau_motion - - pair_timestamps[(row['i_tx'], row['i_rx'])] = row.copy() - - return pair_timestamps, rows - - - -def build_corrected_peaks(peaks_in: Dict[Tuple[int, int], List[Dict]], - pair_timestamps: Dict[Tuple[int, int], Dict]) -> Dict[Tuple[int, int], List[Dict]]: - """ - Формирует словарь corrected_peaks с motion-aware поправками. - - Для каждого пика добавляет: - tau_raw, z_app_raw - tau_corr, z_corr - dz_motion, dtau_motion - - Важно: - z_corr = v * tau_corr / 2 - потому что ось z_app в текущем пайплайне — это apparent depth. - """ - corrected = {} - - for key, peak_list in peaks_in.items(): - if key not in pair_timestamps: - raise KeyError(f"Нет временной информации для пары {key}") - - info = pair_timestamps[key] - dz_motion = info['dz_motion_m'] - dtau_motion = info['dtau_motion_s'] - - corrected_list = [] - for pk in peak_list: - tau_raw = float(pk['tau']) - z_raw = float(pk['z_app']) - # dz_motion here is defined as a correction to the apparent depth itself: - # z_corr = z_raw + dz_motion. - # Therefore tau must be corrected with the same sign. - tau_corr = tau_raw + dtau_motion - z_corr = 0.5 * v * tau_corr - - pk_corr = dict(pk) - pk_corr.update({ - 'tau_raw': tau_raw, - 'z_app_raw': z_raw, - 'tau_corr': tau_corr, - 'z_corr': z_corr, - 'dz_motion': dz_motion, - 'dtau_motion': dtau_motion, - }) - corrected_list.append(pk_corr) - - corrected[key] = corrected_list - - return corrected - - - -pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG) -corrected_peaks = build_corrected_peaks(peaks, pair_timestamps) - - - -# ══════════════════════════════════════════════════════ -# 9. MOTION-AWARE IMAGE BUILD FROM CORRECTED PEAKS -# ══════════════════════════════════════════════════════ - -""" -Эта ячейка строит motion-aware картинку, используя corrected_peaks из блока выше. - -Что меняется относительно статического продакшн-пайплайна: -- в аккумуляторе используется tau_corr вместо tau -- в apparent-depth логике CLEAN используется z_corr вместо z_app -- score считается по corrected пикам - -Исходные A-сканы остаются теми же, но на графике ниже можно показывать уже -motion-corrected положения пиков. -""" - - -def _peak_in_work_depth(pk): - return MIN_DEPTH <= pk['z_corr'] <= MAX_DEPTH - - - -def build_accumulator_motion(corrected_peaks_in, exclude_z_ranges): - acc = np.zeros_like(XX) - for i in range(N_tx): - for j in range(N_rx): - for pk in corrected_peaks_in[(i, j)]: - if not _peak_in_work_depth(pk): - continue - if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges): - continue - r_total = v * pk['tau_corr'] - residual = R_tx_grid[i] + R_rx_grid[j] - r_total - shell = np.exp(-0.5 * (residual / SHELL_SIGMA)**2) - acc += shell * pk['snr_comp'] - return acc - - - -def count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, exclude_z_ranges): - count = 0 - for i in range(N_tx): - for j in range(N_rx): - for pk in corrected_peaks_in[(i, j)]: - if not _peak_in_work_depth(pk): - continue - if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges): - continue - Rt = np.sqrt((x_est - x_tx[i])**2 + z_est**2) - Rr = np.sqrt((x_est - x_rx[j])**2 + z_est**2) - if abs(Rt + Rr - v * pk['tau_corr']) < SHELL_SIGMA * 6: - count += 1 - break - return count - - - -def clean_find_motion(corrected_peaks_in, n_search=10, suppress_r_cm=7, thresh_frac=0.05): - dx = x_grid[1] - x_grid[0] - dz = z_grid[1] - z_grid[0] - rpx = int(suppress_r_cm / 100 / dx) - rpz = int(suppress_r_cm / 100 / dz) - - excl_z = [] - found_motion = [] - acc_initial = build_accumulator_motion(corrected_peaks_in, []) - - for step in range(n_search): - acc = build_accumulator_motion(corrected_peaks_in, excl_z) - acc_s = gaussian_filter(acc, sigma=3) - - if acc_s.max() < thresh_frac * acc_initial.max(): - break - - iz, ix = np.unravel_index(acc_s.argmax(), acc_s.shape) - x_est, z_est = find_centroid(acc_s, iz, ix, rpz, rpx) - score = count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, excl_z) - found_motion.append({'x': x_est, 'z': z_est, 'score': score}) - - matched = [pk['z_corr'] - for i in range(N_tx) for j in range(N_rx) - for pk in corrected_peaks_in[(i, j)] - if _peak_in_work_depth(pk) - and not any(lo <= pk['z_corr'] <= hi for lo, hi in excl_z) - and abs(np.sqrt((x_est - x_tx[i])**2 + z_est**2) + - np.sqrt((x_est - x_rx[j])**2 + z_est**2) - - v * pk['tau_corr']) < SHELL_SIGMA * 3] - - if matched: - margin = SHELL_SIGMA * 1.0 - excl_z.append((min(matched) - margin, max(matched) + margin)) - - return found_motion, acc_initial - - -if MODE == 'point': - found_motion, accum_motion = clean_find_motion(corrected_peaks, n_search=MAX_OBJECTS) - - -# ─── График 2: motion-aware карта накопления ─────────────────────── -fig, ax = plt.subplots(figsize=(12, 7)) -acc_motion_s = gaussian_filter(accum_motion, sigma=3) -im = ax.imshow( - acc_motion_s, - extent=[x_grid[0]*100, x_grid[-1]*100, z_grid[-1]*100, z_grid[0]*100], - aspect='auto', origin='upper', cmap='hot', - vmin=acc_motion_s.max()*0.45, vmax=acc_motion_s.max()*0.95, -) -plt.colorbar(im, ax=ax, label='Накопленный вес (motion-aware)') -ax.plot(x_tx*100, np.zeros(N_tx), 'r^', ms=10, label='Tx', zorder=5) -ax.plot(x_rx*100, np.zeros(N_rx), 'bv', ms=10, label='Rx', zorder=5) -for obj in found_motion: - lbl = f"score={obj['score']}/{N_pairs}" - ax.plot(obj['x']*100, obj['z']*100, 'wD', ms=9, zorder=11, markeredgecolor='black', mew=1.2) - ax.annotate(lbl, (obj['x']*100, obj['z']*100), textcoords='offset points', xytext=(6, 4), - fontsize=8, color='white', bbox=dict(boxstyle='round,pad=0.2', fc='black', alpha=0.5)) -ax.plot([], [], 'wD', ms=9, markeredgecolor='k', mew=1.2, label='Найденные объекты') -ax.set_xlabel('X [см]') -ax.set_ylabel('Глубина Z [см]') -ax.set_title('Motion-aware карта накопления эллипсов') -ax.set_xlim(x_grid[0]*100, x_grid[-1]*100) -ax.set_ylim(z_grid[-1]*100, z_grid[0]*100) -ax.legend(loc='lower right', fontsize=9) -ax.grid(alpha=0.25) -ax.invert_yaxis() -plt.tight_layout() \ No newline at end of file diff --git a/data_acq_and_processing/common_cpp/config/include/run_config.hpp b/data_acq_and_processing/common_cpp/config/include/run_config.hpp index 74aa41a..9a23ccd 100644 --- a/data_acq_and_processing/common_cpp/config/include/run_config.hpp +++ b/data_acq_and_processing/common_cpp/config/include/run_config.hpp @@ -129,7 +129,6 @@ struct GprRxGeometry { struct GprConfig { // Stable collection-level GPR processing settings. - std::string mode = "point"; float relative_permittivity = 1.0F; std::vector tx_geometry{}; std::vector rx_geometry{}; diff --git a/data_acq_and_processing/common_cpp/config/src/run_config.cpp b/data_acq_and_processing/common_cpp/config/src/run_config.cpp index 7d61ea5..e5932e2 100644 --- a/data_acq_and_processing/common_cpp/config/src/run_config.cpp +++ b/data_acq_and_processing/common_cpp/config/src/run_config.cpp @@ -271,9 +271,6 @@ void validate_combo_key(const ipc::ComboKey& key, const RunConfig& config) { } void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) { - if (config.mode != "point" && config.mode != "extended") { - throw std::runtime_error("gpr.mode must be either 'point' or 'extended'"); - } if (!(config.relative_permittivity > 0.0F)) { throw std::runtime_error("gpr.relative_permittivity must be > 0"); } @@ -302,7 +299,6 @@ void validate_gpr_config(const GprConfig& config, const RunConfig& run_config) { [[nodiscard]] auto parse_gpr_config(const Json& object, const RunConfig& run_config) -> GprConfig { const auto* gpr_obj = as_object(object, "gpr"); GprConfig config{}; - config.mode = optional_string(*gpr_obj, "mode", "point"); config.relative_permittivity = optional_f32(*gpr_obj, "relative_permittivity", 1.0F); if (const auto* tx_value = optional_field(*gpr_obj, "tx_geometry"); tx_value != nullptr) { 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 38afe31..03805f7 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 @@ -2,7 +2,6 @@ #include #include -#include #include #include @@ -31,15 +30,13 @@ struct ProcessingLiveConfig { std::vector gpr_output_positions{}; float gpr_min_depth_m = 2.0F; float gpr_max_depth_m = 14.0F; - float gpr_comp_power = 0.2F; + float gpr_range_comp_power = 0.28F; + float gpr_angle_comp_power = 0.10F; float gpr_start_freq_mhz = 3000.0F; float gpr_stop_freq_mhz = 6000.0F; - float gpr_speed_m_s = 0.0F; - float gpr_look_angle_deg = 0.0F; - float gpr_snr_thresh = 4.5F; - float gpr_snr_comp_max = 25.0F; bool gpr_background_subtract_enabled = true; std::uint32_t gpr_background_mean_count = 10U; + bool gpr_remove_sidelobe_objects_enabled = true; std::uint64_t history_command_seq = 0; HistoryCommand history_command = HistoryCommand::None; }; @@ -53,12 +50,12 @@ class ProcessingLiveConfigLoader { [[nodiscard]] auto revision() const -> std::uint64_t; private: - [[nodiscard]] auto read_from_file() const -> ProcessingLiveConfig; + [[nodiscard]] auto read_file_text() const -> std::string; std::string path_{}; ProcessingLiveConfig current_{}; - std::filesystem::file_time_type last_write_time_{}; - bool has_last_write_time_ = false; + std::string last_json_text_{}; + bool has_last_json_text_ = false; std::uint64_t revision_ = 0; std::chrono::steady_clock::time_point next_check_at_{}; }; 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 1968ba5..82e7f32 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 @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -181,11 +182,17 @@ using Json = nlohmann::json; } config.gpr_max_depth_m = static_cast(found->get()); } - if (const auto found = root.find("gpr_comp_power"); found != root.end()) { + if (const auto found = root.find("gpr_range_comp_power"); found != root.end()) { if (!found->is_number()) { - throw std::runtime_error("processing.gpr_comp_power must be number"); + throw std::runtime_error("processing.gpr_range_comp_power must be number"); } - config.gpr_comp_power = static_cast(found->get()); + config.gpr_range_comp_power = static_cast(found->get()); + } + if (const auto found = root.find("gpr_angle_comp_power"); found != root.end()) { + if (!found->is_number()) { + throw std::runtime_error("processing.gpr_angle_comp_power must be number"); + } + config.gpr_angle_comp_power = static_cast(found->get()); } if (const auto found = root.find("gpr_start_freq_mhz"); found != root.end()) { if (!found->is_number()) { @@ -199,30 +206,6 @@ using Json = nlohmann::json; } config.gpr_stop_freq_mhz = static_cast(found->get()); } - 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"); - } - config.gpr_speed_m_s = static_cast(found->get()); - } - if (const auto found = root.find("gpr_look_angle_deg"); found != root.end()) { - if (!found->is_number()) { - throw std::runtime_error("processing.gpr_look_angle_deg must be number"); - } - config.gpr_look_angle_deg = static_cast(found->get()); - } - if (const auto found = root.find("gpr_snr_thresh"); found != root.end()) { - if (!found->is_number()) { - throw std::runtime_error("processing.gpr_snr_thresh must be number"); - } - config.gpr_snr_thresh = static_cast(found->get()); - } - if (const auto found = root.find("gpr_snr_comp_max"); found != root.end()) { - if (!found->is_number()) { - throw std::runtime_error("processing.gpr_snr_comp_max must be number"); - } - config.gpr_snr_comp_max = static_cast(found->get()); - } if (const auto found = root.find("gpr_background_subtract_enabled"); found != root.end()) { if (!found->is_boolean()) { throw std::runtime_error("processing.gpr_background_subtract_enabled must be bool"); @@ -232,6 +215,12 @@ using Json = nlohmann::json; if (const auto found = root.find("gpr_background_mean_count"); found != root.end()) { config.gpr_background_mean_count = parse_u32_number(*found, "processing.gpr_background_mean_count"); } + if (const auto found = root.find("gpr_remove_sidelobe_objects_enabled"); found != root.end()) { + if (!found->is_boolean()) { + throw std::runtime_error("processing.gpr_remove_sidelobe_objects_enabled must be bool"); + } + config.gpr_remove_sidelobe_objects_enabled = found->get(); + } if (const auto found = root.find("history_command_seq"); found != root.end()) { config.history_command_seq = parse_u64_number(*found, "processing.history_command_seq"); } @@ -269,20 +258,20 @@ auto ProcessingLiveConfigLoader::refresh_if_needed() -> ProcessingLiveConfig { } next_check_at_ = now + std::chrono::milliseconds(100); - std::error_code time_error{}; - const auto file_time = std::filesystem::last_write_time(path_, time_error); - if (time_error) { - return current_; - } - - if (has_last_write_time_ && file_time == last_write_time_) { + std::error_code file_error{}; + if (!std::filesystem::exists(path_, file_error) || file_error) { return current_; } try { - current_ = read_from_file(); - last_write_time_ = file_time; - has_last_write_time_ = true; + const auto json_text = read_file_text(); + if (has_last_json_text_ && json_text == last_json_text_) { + return current_; + } + + current_ = parse_live_config(json_text, path_); + last_json_text_ = json_text; + has_last_json_text_ = true; ++revision_; } catch (const std::exception& error) { std::cerr << "data_processor warning: failed to refresh live processing config: " << error.what() << '\n'; @@ -291,7 +280,7 @@ auto ProcessingLiveConfigLoader::refresh_if_needed() -> ProcessingLiveConfig { return current_; } -auto ProcessingLiveConfigLoader::read_from_file() const -> ProcessingLiveConfig { +auto ProcessingLiveConfigLoader::read_file_text() const -> std::string { std::ifstream stream(path_); if (!stream.is_open()) { throw std::runtime_error("Failed to open live processing config: " + path_); @@ -299,7 +288,7 @@ auto ProcessingLiveConfigLoader::read_from_file() const -> ProcessingLiveConfig std::stringstream buffer{}; buffer << stream.rdbuf(); - return parse_live_config(buffer.str(), path_); + return buffer.str(); } } // namespace radar::processing 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 0f4c83c..7017fd6 100644 --- a/data_acq_and_processing/processing/processors/src/gpr_processor.cpp +++ b/data_acq_and_processing/processing/processors/src/gpr_processor.cpp @@ -7,7 +7,10 @@ #include #include #include +#include +#include #include +#include #include #include #include @@ -18,58 +21,38 @@ namespace { constexpr double kPi = 3.14159265358979323846; constexpr double kSpeedOfLightMetersPerSec = 299'792'458.0; -constexpr double kAccumulatorXMarginM = 2.0; -constexpr double kAccumulatorZMinM = 0.20; +constexpr double kXMarginM = 2.0; +constexpr double kGridZMinM = 0.1; constexpr std::size_t kGridWidth = 300U; constexpr std::size_t kGridHeight = 300U; -constexpr double kGaussianSigma = 3.0; -constexpr double kCleanSuppressRadiusM = 0.07; -constexpr double kCleanThresholdFrac = 0.05; -constexpr std::size_t kMaxObjects = 15U; -constexpr double kExtendedThresholdFrac = 0.75; -constexpr double kExtendedMinAreaCm2 = 2.0; +constexpr std::size_t kAscanOversample = 8U; + +constexpr double kRangeWeightMax = 5.0; +constexpr double kAngleWeightMax = 2.0; +constexpr double kTotalWeightMax = 8.0; +constexpr double kCompensationReferenceDepthM = 3.0; + +constexpr double kSmoothSigma = 1.5; +constexpr std::size_t kMaxObjects = 10U; +constexpr double kObjectMinFrac = 0.35; +constexpr double kRegionThresholdFrac = 0.75; +constexpr double kSuppressThresholdFrac = 0.20; +constexpr double kSuppressRadiusXM = 0.80; +constexpr double kSuppressRadiusZM = 0.40; +constexpr double kMinRegionAreaCm2 = 10.0; + +constexpr double kCenterRadiusXM = 0.60; +constexpr double kCenterRadiusZM = 0.25; +constexpr double kCenterThresholdFrac = 0.88; +constexpr double kCenterWeightPower = 2.0; + +constexpr double kSidelobeRangeRmsToleranceM = 0.20; +constexpr double kSidelobeMinDxM = 0.35; +constexpr double kSidelobeMaxDzM = 0.70; +constexpr double kSidelobeMaxRelativePeak = 0.85; using PairKey = std::uint64_t; -struct SelectedTrace { - ipc::ComboKey combo{}; - std::uint32_t tx_local_index = 0U; - std::uint32_t rx_local_index = 0U; - std::size_t run_order = 0U; - std::vector frequency_hz{}; - std::vector> s21{}; -}; - -struct AscanResult { - std::vector time_s{}; - std::vector depth_m{}; - std::vector amplitude{}; - double bandwidth_hz = 0.0; -}; - -struct PeakRecord { - double z_app = 0.0; - double tau = 0.0; - double tau_corr = 0.0; - double z_corr = 0.0; - double snr_raw = 0.0; - double snr_comp = 0.0; -}; - -struct PointRecord { - double x_m = 0.0; - double z_m = 0.0; - double score = 0.0; -}; - -struct RegionRecord { - double x_m = 0.0; - double z_m = 0.0; - double score = 0.0; - double pixel_count = 0.0; - std::vector mask{}; -}; - struct GeometrySelection { std::vector input_positions{}; std::vector output_positions{}; @@ -84,10 +67,20 @@ struct BackgroundAccumulator { std::size_t count = 0U; }; -struct PairTiming { - double dt_ref_s = 0.0; - double dz_motion_m = 0.0; - double dtau_motion_s = 0.0; +struct SelectedTrace { + ipc::ComboKey combo{}; + std::uint32_t tx_local_index = 0U; + std::uint32_t rx_local_index = 0U; + std::size_t run_order = 0U; + std::vector frequency_hz{}; + std::vector> s21{}; +}; + +struct AscanResult { + std::vector time_s{}; + std::vector> samples{}; + double dt_s = 0.0; + double bandwidth_hz = 0.0; }; struct GridDefinition { @@ -97,9 +90,29 @@ struct GridDefinition { std::vector> rx_distance_grids{}; }; -enum class PeakDomain { - Apparent, - Corrected, +struct BpMap { + std::vector image{}; + std::vector> coherent{}; +}; + +struct ObjectRecord { + std::size_t index = 0U; + double x_peak_m = 0.0; + double z_peak_m = 0.0; + double x_m = 0.0; + double z_m = 0.0; + double x_region_m = 0.0; + double z_region_m = 0.0; + double peak = 0.0; + double area_cm2 = 0.0; + double center_area_cm2 = 0.0; + double mean_value = 0.0; + double sum_value = 0.0; + std::vector region_mask{}; + std::vector center_mask{}; + bool sidelobe_candidate = false; + std::size_t sidelobe_parent = 0U; + double sidelobe_range_rms_m = std::numeric_limits::quiet_NaN(); }; [[nodiscard]] auto make_pair_key(std::uint32_t tx_local_index, std::uint32_t rx_local_index) -> PairKey { @@ -170,6 +183,22 @@ void fft_inplace(std::vector>& values, bool inverse) { } } +[[nodiscard]] auto median_copy(std::vector values) -> double { + if (values.empty()) { + return 0.0; + } + + const auto middle = values.begin() + static_cast(values.size() / 2U); + std::nth_element(values.begin(), middle, values.end()); + double median = *middle; + if ((values.size() % 2U) == 0U) { + const auto lower_middle = values.begin() + static_cast((values.size() / 2U) - 1U); + std::nth_element(values.begin(), lower_middle, values.end()); + median = 0.5 * (median + *lower_middle); + } + return median; +} + [[nodiscard]] auto build_axis(double min_value, double max_value, std::size_t count) -> std::vector { std::vector axis{}; if (count == 0U) { @@ -202,6 +231,23 @@ void fft_inplace(std::vector>& values, bool inverse) { return static_cast(value); } +[[nodiscard]] auto max_value(const std::vector& values) -> double { + if (values.empty()) { + return 0.0; + } + return *std::max_element(values.begin(), values.end()); +} + +void normalize_in_place(std::vector& values) { + const double maximum = max_value(values); + if (!(maximum > 0.0)) { + return; + } + for (auto& value : values) { + value /= maximum; + } +} + [[nodiscard]] auto build_gaussian_kernel(double sigma) -> std::vector { if (!(sigma > 0.0)) { return {1.0}; @@ -263,96 +309,34 @@ void fft_inplace(std::vector>& values, bool inverse) { return output; } -[[nodiscard]] auto median_copy(std::vector values) -> double { - if (values.empty()) { - return 0.0; - } - - const auto middle = values.begin() + static_cast(values.size() / 2U); - std::nth_element(values.begin(), middle, values.end()); - double median = *middle; - if ((values.size() % 2U) == 0U) { - const auto lower_middle = values.begin() + static_cast((values.size() / 2U) - 1U); - std::nth_element(values.begin(), lower_middle, values.end()); - median = 0.5 * (median + *lower_middle); - } - return median; +[[nodiscard]] auto contains_u32(const std::vector& values, std::uint32_t value) -> bool { + return std::find(values.begin(), values.end(), value) != values.end(); } -[[nodiscard]] auto find_peak_indices( - const std::vector& values, - std::size_t start_index, - std::size_t stop_index, - double threshold, - std::size_t min_distance -) -> std::vector { - if (stop_index <= start_index + 2U) { - return {}; +[[nodiscard]] auto selected_positions( + const std::vector& requested, + std::vector available +) -> std::vector { + std::sort(available.begin(), available.end()); + available.erase(std::unique(available.begin(), available.end()), available.end()); + if (requested.empty()) { + return available; } - std::vector candidates{}; - for (std::size_t index = start_index + 1U; index + 1U < stop_index; ++index) { - if (values[index] < threshold) { - continue; - } - if (values[index] <= values[index - 1U]) { - continue; - } - if (values[index] < values[index + 1U]) { - continue; - } - candidates.push_back(index); - } - - std::sort(candidates.begin(), candidates.end(), [&](const std::size_t left, const std::size_t right) { - return values[left] > values[right]; - }); - - std::vector selected{}; - for (const auto index : candidates) { - bool keep = true; - for (const auto accepted : selected) { - const auto distance = accepted > index ? accepted - index : index - accepted; - if (distance < min_distance) { - keep = false; - break; - } - } - if (keep) { - selected.push_back(index); + std::vector result{}; + for (const auto value : requested) { + if (contains_u32(available, value) && !contains_u32(result, value)) { + result.push_back(value); } } - - std::sort(selected.begin(), selected.end()); - return selected; -} - -[[nodiscard]] auto attenuation_at_depth( - std::size_t tx_index, - std::size_t rx_index, - double z_app, - const std::vector& x_tx, - const std::vector& x_rx -) -> double { - const double xc = (x_tx[tx_index] + x_rx[rx_index]) / 2.0; - const double r_tx = std::sqrt(std::pow(xc - x_tx[tx_index], 2.0) + std::pow(z_app, 2.0)); - const double r_rx = std::sqrt(std::pow(xc - x_rx[rx_index], 2.0) + std::pow(z_app, 2.0)); - const double geo = 1.0 / ((r_tx * r_rx) + 1e-12); - const double pat = - std::pow(z_app / (r_tx + 1e-12), 2.0) * std::pow(z_app / (r_rx + 1e-12), 2.0); - return geo * pat + 1e-30; -} - -[[nodiscard]] auto lower_bound_index(const std::vector& axis, double value) -> std::size_t { - return static_cast(std::distance(axis.begin(), std::lower_bound(axis.begin(), axis.end(), value))); + std::sort(result.begin(), result.end()); + return result; } [[nodiscard]] auto build_geometry_selection( const config::RunConfig& run_config, const ProcessingLiveConfig& live_config ) -> GeometrySelection { - GeometrySelection selection{}; - std::unordered_map tx_x_by_pos{}; for (const auto& entry : run_config.gpr.tx_geometry) { tx_x_by_pos[entry.output_pos] = static_cast(entry.x_m); @@ -363,24 +347,6 @@ void fft_inplace(std::vector>& values, bool inverse) { rx_x_by_pos[entry.input_pos] = static_cast(entry.x_m); } - auto select_positions = [](const std::vector& requested, std::vector available) { - std::sort(available.begin(), available.end()); - available.erase(std::unique(available.begin(), available.end()), available.end()); - if (requested.empty()) { - return available; - } - - std::vector result{}; - for (const auto value : requested) { - if (std::find(available.begin(), available.end(), value) != available.end()) { - result.push_back(value); - } - } - std::sort(result.begin(), result.end()); - result.erase(std::unique(result.begin(), result.end()), result.end()); - return result; - }; - std::vector available_outputs{}; available_outputs.reserve(tx_x_by_pos.size()); for (const auto& [position, _] : tx_x_by_pos) { @@ -393,8 +359,9 @@ void fft_inplace(std::vector>& values, bool inverse) { available_inputs.push_back(position); } - selection.output_positions = select_positions(live_config.gpr_output_positions, std::move(available_outputs)); - selection.input_positions = select_positions(live_config.gpr_input_positions, std::move(available_inputs)); + GeometrySelection selection{}; + selection.output_positions = selected_positions(live_config.gpr_output_positions, std::move(available_outputs)); + selection.input_positions = selected_positions(live_config.gpr_input_positions, std::move(available_inputs)); selection.x_tx.reserve(selection.output_positions.size()); for (std::size_t index = 0U; index < selection.output_positions.size(); ++index) { @@ -497,7 +464,6 @@ void validate_collection_trace_order( } [[nodiscard]] auto collect_selected_traces( - const config::RunConfig& run_config, const ipc::PreprocessedCollection& collection, const GeometrySelection& selection, const std::unordered_map>>& background_mean @@ -513,6 +479,9 @@ void validate_collection_trace_order( if (output_it == selection.output_local_by_pos.end() || input_it == selection.input_local_by_pos.end()) { continue; } + if (trace.frequency_hz.size() != trace.s21.size()) { + continue; + } SelectedTrace selected{}; selected.combo = trace.combo; @@ -524,15 +493,13 @@ void validate_collection_trace_order( selected.frequency_hz.push_back(static_cast(value)); } - selected.s21.reserve(trace.s21.size()); const auto key = make_pair_key(selected.tx_local_index, selected.rx_local_index); if (!seen_keys.insert(key).second) { - throw std::runtime_error( - "Motion-aware GPR requires unique selected combos in run.combos; duplicate combo detected: " + - combo_to_string(run_config.run_combos[trace_index]) - ); + throw std::runtime_error("GPR requires unique selected combos; duplicate combo: " + combo_to_string(trace.combo)); } + const auto background_it = background_mean.find(key); + selected.s21.reserve(trace.s21.size()); for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) { std::complex sample(trace.s21[sample_index].re, trace.s21[sample_index].im); if (background_it != background_mean.end() && background_it->second.size() == trace.s21.size()) { @@ -550,21 +517,20 @@ void validate_collection_trace_order( [[nodiscard]] auto compute_ascan( const SelectedTrace& trace, double start_hz, - double stop_hz, - double velocity_mps + double stop_hz ) -> AscanResult { AscanResult result{}; if (trace.frequency_hz.size() != trace.s21.size()) { return result; } + const double low_hz = std::min(start_hz, stop_hz); + const double high_hz = std::max(start_hz, stop_hz); std::vector frequency_hz{}; std::vector> s21{}; frequency_hz.reserve(trace.frequency_hz.size()); s21.reserve(trace.s21.size()); - const double low_hz = std::min(start_hz, stop_hz); - const double high_hz = std::max(start_hz, stop_hz); for (std::size_t index = 0U; index < trace.frequency_hz.size(); ++index) { const double frequency_value = trace.frequency_hz[index]; if (frequency_value < low_hz || frequency_value > high_hz) { @@ -578,21 +544,36 @@ void validate_collection_trace_order( return result; } - const std::size_t point_count = frequency_hz.size(); - const double df = (frequency_hz.back() - frequency_hz.front()) / static_cast(point_count - 1U); - if (!(df > 0.0)) { + std::vector df_values{}; + df_values.reserve(frequency_hz.size() - 1U); + for (std::size_t index = 1U; index < frequency_hz.size(); ++index) { + const double df = frequency_hz[index] - frequency_hz[index - 1U]; + if (!(df > 0.0)) { + return result; + } + df_values.push_back(df); + } + + const double df_hz = median_copy(std::move(df_values)); + if (!(df_hz > 0.0)) { return result; } - const auto start_bin = static_cast(std::llround(frequency_hz.front() / df)); + const auto start_bin = static_cast(std::llround(frequency_hz.front() / df_hz)); if (start_bin < 0) { return result; } + const std::size_t point_count = frequency_hz.size(); const auto start_index = static_cast(start_bin); const std::size_t min_fft_len = 2U * (start_index + point_count - 1U); - const std::size_t fft_len = next_power_of_two(min_fft_len); - if (fft_len < min_fft_len || start_index > fft_len || point_count > (fft_len - start_index)) { + const std::size_t base_fft_len = next_power_of_two(min_fft_len); + if (base_fft_len < min_fft_len || base_fft_len > (std::numeric_limits::max() / kAscanOversample)) { + return result; + } + + const std::size_t fft_len = base_fft_len * kAscanOversample; + if (start_index > fft_len || point_count > (fft_len - start_index)) { return result; } @@ -607,16 +588,12 @@ void validate_collection_trace_order( fft_inplace(spectrum, true); result.bandwidth_hz = frequency_hz.back() - frequency_hz.front(); - const double dt = 1.0 / (static_cast(fft_len) * df); + result.dt_s = 1.0 / (static_cast(fft_len) * df_hz); + result.samples = std::move(spectrum); result.time_s.resize(fft_len, 0.0); - result.depth_m.resize(fft_len, 0.0); - result.amplitude.resize(fft_len, 0.0); for (std::size_t index = 0U; index < fft_len; ++index) { - result.time_s[index] = static_cast(index) * dt; - result.depth_m[index] = result.time_s[index] * velocity_mps / 2.0; - result.amplitude[index] = std::abs(spectrum[index]); + result.time_s[index] = static_cast(index) * result.dt_s; } - return result; } @@ -626,42 +603,35 @@ void validate_collection_trace_order( double max_depth_m ) -> GridDefinition { GridDefinition grid{}; - if (x_tx.empty() || x_rx.empty() || !(max_depth_m > kAccumulatorZMinM)) { + if (x_tx.empty() || x_rx.empty() || !(max_depth_m > kGridZMinM)) { return grid; } const auto [tx_min_it, tx_max_it] = std::minmax_element(x_tx.begin(), x_tx.end()); const auto [rx_min_it, rx_max_it] = std::minmax_element(x_rx.begin(), x_rx.end()); - const double x_min = std::min(*tx_min_it, *rx_min_it) - kAccumulatorXMarginM; - const double x_max = std::max(*tx_max_it, *rx_max_it) + kAccumulatorXMarginM; + const double x_min = std::min(*tx_min_it, *rx_min_it) - kXMarginM; + const double x_max = std::max(*tx_max_it, *rx_max_it) + kXMarginM; grid.x_grid = build_axis(x_min, x_max, kGridWidth); - grid.z_grid = build_axis(kAccumulatorZMinM, max_depth_m, kGridHeight); + grid.z_grid = build_axis(kGridZMinM, max_depth_m, kGridHeight); const std::size_t cell_count = grid.x_grid.size() * grid.z_grid.size(); grid.tx_distance_grids.assign(x_tx.size(), std::vector(cell_count, 0.0)); grid.rx_distance_grids.assign(x_rx.size(), std::vector(cell_count, 0.0)); - for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) { - for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) { - const double z_value = grid.z_grid[row]; - for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) { - const double x_value = grid.x_grid[col]; - const auto cell_index = (row * grid.x_grid.size()) + col; - grid.tx_distance_grids[tx_index][cell_index] = - std::sqrt(std::pow(x_value - x_tx[tx_index], 2.0) + std::pow(z_value, 2.0)); - } - } - } + for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) { + const double z_value = grid.z_grid[row]; + for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) { + const double x_value = grid.x_grid[col]; + const auto cell_index = (row * grid.x_grid.size()) + col; - for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) { - for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) { - const double z_value = grid.z_grid[row]; - for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) { - const double x_value = grid.x_grid[col]; - const auto cell_index = (row * grid.x_grid.size()) + col; + for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) { + grid.tx_distance_grids[tx_index][cell_index] = + std::hypot(x_value - x_tx[tx_index], z_value); + } + for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) { grid.rx_distance_grids[rx_index][cell_index] = - std::sqrt(std::pow(x_value - x_rx[rx_index], 2.0) + std::pow(z_value, 2.0)); + std::hypot(x_value - x_rx[rx_index], z_value); } } } @@ -669,220 +639,538 @@ void validate_collection_trace_order( return grid; } -[[nodiscard]] auto is_excluded(double z_value, const std::vector>& ranges) -> bool { - for (const auto& [low, high] : ranges) { - if (z_value >= low && z_value <= high) { - return true; - } +[[nodiscard]] auto interpolate_complex(const AscanResult& ascan, double tau_s) -> std::complex { + if (ascan.samples.empty() || !(ascan.dt_s > 0.0) || tau_s < 0.0) { + return {0.0, 0.0}; } - return false; + + const double position = tau_s / ascan.dt_s; + const auto index = static_cast(std::floor(position)); + if (index >= ascan.samples.size()) { + return {0.0, 0.0}; + } + if (index + 1U >= ascan.samples.size()) { + return ascan.samples[index]; + } + + const double frac = position - static_cast(index); + return (ascan.samples[index] * (1.0 - frac)) + (ascan.samples[index + 1U] * frac); } -[[nodiscard]] auto peak_depth_for_domain(const PeakRecord& peak, PeakDomain domain) -> double { - return domain == PeakDomain::Corrected ? peak.z_corr : peak.z_app; +[[nodiscard]] auto attenuation_components( + double r_tx, + double r_rx, + double z_m +) -> std::pair { + const double geo = 1.0 / ((r_tx * r_rx) + 1e-12); + const double angle = + std::pow(z_m / (r_tx + 1e-12), 2.0) * + std::pow(z_m / (r_rx + 1e-12), 2.0); + return {geo + 1e-30, angle + 1e-30}; } -[[nodiscard]] auto peak_tau_for_domain(const PeakRecord& peak, PeakDomain domain) -> double { - return domain == PeakDomain::Corrected ? peak.tau_corr : peak.tau; +[[nodiscard]] auto attenuation_components_at_ref_depth( + std::uint32_t tx_index, + std::uint32_t rx_index, + const std::vector& x_tx, + const std::vector& x_rx +) -> std::pair { + const double x_center = 0.5 * (x_tx[tx_index] + x_rx[rx_index]); + const double r_tx = std::hypot(x_center - x_tx[tx_index], kCompensationReferenceDepthM); + const double r_rx = std::hypot(x_center - x_rx[rx_index], kCompensationReferenceDepthM); + return attenuation_components(r_tx, r_rx, kCompensationReferenceDepthM); } -[[nodiscard]] auto build_motion_timing_by_pair( - const std::vector& traces, - std::size_t total_combo_count, - std::uint64_t capture_start_ns, - std::uint64_t capture_end_ns, - const ProcessingLiveConfig& live_config, - double velocity_mps -) -> std::unordered_map { - std::unordered_map timing_by_pair{}; - timing_by_pair.reserve(traces.size()); +[[nodiscard]] auto compensation_weight( + double r_tx, + double r_rx, + double z_m, + double geo_ref, + double angle_ref, + double range_power, + double angle_power +) -> double { + const auto [geo, angle] = attenuation_components(r_tx, r_rx, z_m); + const double geo_norm = geo / geo_ref; + const double angle_norm = angle / angle_ref; - const double speed_m_s = static_cast(live_config.gpr_speed_m_s); - if (!(std::abs(speed_m_s) > 1e-12)) { - for (const auto& trace : traces) { - timing_by_pair.emplace( - make_pair_key(trace.tx_local_index, trace.rx_local_index), - PairTiming{} - ); - } - return timing_by_pair; - } - - if (total_combo_count == 0U) { - throw std::runtime_error("Motion-aware GPR requires at least one run combo"); - } - if (capture_end_ns <= capture_start_ns) { - throw std::runtime_error( - "Motion-aware GPR requires valid capture_start_ns/capture_end_ns metadata when gpr_speed_m_s is non-zero" - ); - } - - const double capture_span_s = static_cast(capture_end_ns - capture_start_ns) * 1e-9; - const double slot_duration_s = capture_span_s / static_cast(total_combo_count); - if (!(slot_duration_s > 0.0)) { - throw std::runtime_error( - "Motion-aware GPR requires positive collection capture span when gpr_speed_m_s is non-zero" - ); - } - - const double t_ref_s = 0.5 * capture_span_s; - const double cos_theta = std::cos((static_cast(live_config.gpr_look_angle_deg) * kPi) / 180.0); - for (const auto& trace : traces) { - const double t_center_s = (static_cast(trace.run_order) + 0.5) * slot_duration_s; - const double dt_ref_s = t_center_s - t_ref_s; - const double dz_motion_m = speed_m_s * dt_ref_s * cos_theta; - const double dtau_motion_s = (2.0 * dz_motion_m) / velocity_mps; - timing_by_pair.emplace( - make_pair_key(trace.tx_local_index, trace.rx_local_index), - PairTiming{ - .dt_ref_s = dt_ref_s, - .dz_motion_m = dz_motion_m, - .dtau_motion_s = dtau_motion_s, - } - ); - } - - return timing_by_pair; + const double range_weight = std::clamp( + 1.0 / (std::pow(geo_norm, range_power) + 1e-12), + 0.0, + kRangeWeightMax + ); + const double angle_weight = std::clamp( + 1.0 / (std::pow(angle_norm, angle_power) + 1e-12), + 0.0, + kAngleWeightMax + ); + return std::clamp(range_weight * angle_weight, 0.0, kTotalWeightMax); } -void apply_motion_correction( - std::unordered_map>& peaks_by_pair, - const std::unordered_map& timing_by_pair, - double velocity_mps -) { - for (auto& [key, peaks] : peaks_by_pair) { - const auto timing_it = timing_by_pair.find(key); - if (timing_it == timing_by_pair.end()) { - throw std::runtime_error("Missing motion timing for selected GPR combo"); - } - - for (auto& peak : peaks) { - peak.tau_corr = peak.tau + timing_it->second.dtau_motion_s; - peak.z_corr = 0.5 * velocity_mps * peak.tau_corr; - } - } -} - -[[nodiscard]] auto build_accumulator( +[[nodiscard]] auto backproject_coherent( + const std::vector& selected_traces, + const std::unordered_map& ascans_by_pair, const GridDefinition& grid, - const std::unordered_map>& peaks_by_pair, - const std::vector>& exclude_ranges, - double velocity_mps, - double shell_sigma_m, const std::vector& x_tx, const std::vector& x_rx, - PeakDomain domain -) -> std::vector { + double velocity_mps, + double min_depth_m, + double max_depth_m, + double range_power, + double angle_power +) -> BpMap { const std::size_t width = grid.x_grid.size(); const std::size_t height = grid.z_grid.size(); - std::vector accumulator(width * height, 0.0); - if (!(shell_sigma_m > 0.0)) { - return accumulator; - } + const std::size_t cell_count = width * height; - for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) { - for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) { - const auto peak_it = peaks_by_pair.find(make_pair_key(static_cast(tx_index), static_cast(rx_index))); - if (peak_it == peaks_by_pair.end()) { + BpMap result{}; + result.image.assign(cell_count, 0.0); + result.coherent.assign(cell_count, std::complex(0.0, 0.0)); + std::vector contribution_count(cell_count, 0.0); + + 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()) { + continue; + } + + const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth( + trace.tx_local_index, + trace.rx_local_index, + x_tx, + x_rx + ); + const auto& tx_distances = grid.tx_distance_grids[trace.tx_local_index]; + const auto& rx_distances = grid.rx_distance_grids[trace.rx_local_index]; + + for (std::size_t row = 0U; row < height; ++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; } - const auto& tx_grid = grid.tx_distance_grids[tx_index]; - const auto& rx_grid = grid.rx_distance_grids[rx_index]; - for (const auto& peak : peak_it->second) { - if (is_excluded(peak_depth_for_domain(peak, domain), exclude_ranges)) { + 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; + const auto sample = interpolate_complex(ascan_it->second, tau_s); + if (sample == std::complex(0.0, 0.0)) { continue; } - const double range_total = velocity_mps * peak_tau_for_domain(peak, domain); - for (std::size_t cell_index = 0U; cell_index < accumulator.size(); ++cell_index) { - const double residual = tx_grid[cell_index] + rx_grid[cell_index] - range_total; - const double shell = std::exp(-0.5 * std::pow(residual / shell_sigma_m, 2.0)); - accumulator[cell_index] += shell * peak.snr_comp; - } + const double weight = compensation_weight( + r_tx, + r_rx, + z_m, + geo_ref, + angle_ref, + range_power, + angle_power + ); + result.coherent[cell_index] += sample * weight; + contribution_count[cell_index] += 1.0; } } } - return accumulator; + for (std::size_t index = 0U; index < cell_count; ++index) { + if (!(contribution_count[index] > 0.0)) { + continue; + } + result.coherent[index] /= contribution_count[index]; + result.image[index] = std::abs(result.coherent[index]); + } + + return result; } -[[nodiscard]] auto count_agreeing_ellipses( - double x_est, - double z_est, - const std::unordered_map>& peaks_by_pair, - const std::vector>& exclude_ranges, - const std::vector& x_tx, - const std::vector& x_rx, - double velocity_mps, - double shell_sigma_m, - PeakDomain domain -) -> double { - std::size_t count = 0U; - for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) { - for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) { - const auto peak_it = peaks_by_pair.find(make_pair_key(static_cast(tx_index), static_cast(rx_index))); - if (peak_it == peaks_by_pair.end()) { +void apply_depth_gate( + std::vector& image, + const std::vector& z_grid, + std::size_t width, + double min_depth_m, + double max_depth_m +) { + for (std::size_t row = 0U; row < z_grid.size(); ++row) { + const double z_m = z_grid[row]; + if (z_m >= min_depth_m && z_m <= max_depth_m) { + continue; + } + const auto row_offset = row * width; + std::fill(image.begin() + static_cast(row_offset), + image.begin() + static_cast(row_offset + width), + 0.0); + } +} + +[[nodiscard]] auto neighbor_indices( + std::size_t index, + std::size_t width, + std::size_t height +) -> std::vector { + const auto row = index / width; + const auto col = index % width; + std::vector neighbors{}; + neighbors.reserve(8U); + + for (std::ptrdiff_t row_offset = -1; row_offset <= 1; ++row_offset) { + for (std::ptrdiff_t col_offset = -1; col_offset <= 1; ++col_offset) { + if (row_offset == 0 && col_offset == 0) { continue; } - - for (const auto& peak : peak_it->second) { - if (is_excluded(peak_depth_for_domain(peak, domain), exclude_ranges)) { - continue; - } - - const double rt = std::sqrt(std::pow(x_est - x_tx[tx_index], 2.0) + std::pow(z_est, 2.0)); - const double rr = std::sqrt(std::pow(x_est - x_rx[rx_index], 2.0) + std::pow(z_est, 2.0)); - if (std::abs((rt + rr) - (velocity_mps * peak_tau_for_domain(peak, domain))) < shell_sigma_m * 6.0) { - count += 1U; - break; - } + const auto next_row = static_cast(row) + row_offset; + const auto next_col = static_cast(col) + col_offset; + if (next_row < 0 || next_col < 0) { + continue; } + if (next_row >= static_cast(height) || next_col >= static_cast(width)) { + continue; + } + neighbors.push_back((static_cast(next_row) * width) + static_cast(next_col)); } } - return static_cast(count); + return neighbors; } -[[nodiscard]] auto find_centroid( - const std::vector& values, +[[nodiscard]] auto component_containing_peak( + const std::vector& image, std::size_t width, std::size_t height, - std::size_t iz, - std::size_t ix, - std::size_t radius_z, - std::size_t radius_x, - const std::vector& x_grid, - const std::vector& z_grid -) -> std::pair { - if (values.empty()) { - return {x_grid[ix], z_grid[iz]}; + std::size_t peak_index, + double threshold, + const std::vector* window_mask = nullptr +) -> std::vector { + std::vector result(image.size(), 0U); + if (image.empty() || peak_index >= image.size()) { + return result; + } + if (image[peak_index] < threshold || (window_mask != nullptr && (*window_mask)[peak_index] == 0U)) { + result[peak_index] = 1U; + return result; } - const auto row_start = iz > radius_z ? iz - radius_z : 0U; - const auto row_stop = std::min(height, iz + radius_z + 1U); - const auto col_start = ix > radius_x ? ix - radius_x : 0U; - const auto col_stop = std::min(width, ix + radius_x + 1U); + std::queue queue{}; + result[peak_index] = 1U; + queue.push(peak_index); - double weight_sum = 0.0; - double row_weighted_sum = 0.0; - double col_weighted_sum = 0.0; - for (std::size_t row = row_start; row < row_stop; ++row) { - for (std::size_t col = col_start; col < col_stop; ++col) { - const double weight = values[(row * width) + col]; - weight_sum += weight; - row_weighted_sum += static_cast(row) * weight; - col_weighted_sum += static_cast(col) * weight; + while (!queue.empty()) { + const auto current = queue.front(); + queue.pop(); + + for (const auto neighbor : neighbor_indices(current, width, height)) { + if (result[neighbor] != 0U) { + continue; + } + if (window_mask != nullptr && (*window_mask)[neighbor] == 0U) { + continue; + } + if (image[neighbor] < threshold) { + continue; + } + result[neighbor] = 1U; + queue.push(neighbor); } } + return result; +} + +[[nodiscard]] auto mask_count(const std::vector& mask) -> std::size_t { + return static_cast(std::count(mask.begin(), mask.end(), static_cast(1U))); +} + +[[nodiscard]] auto weighted_centroid( + const std::vector& image, + const std::vector& mask, + const GridDefinition& grid, + double threshold +) -> std::pair { + double weight_sum = 0.0; + double x_weighted_sum = 0.0; + double z_weighted_sum = 0.0; + + for (std::size_t index = 0U; index < image.size(); ++index) { + if (mask[index] == 0U) { + continue; + } + double weight = std::max(image[index] - threshold, 0.0); + if (!(weight > 0.0)) { + weight = image[index]; + } + if (!(weight > 0.0)) { + continue; + } + + const auto row = index / grid.x_grid.size(); + const auto col = index % grid.x_grid.size(); + weight_sum += weight; + x_weighted_sum += grid.x_grid[col] * weight; + z_weighted_sum += grid.z_grid[row] * weight; + } + if (!(weight_sum > 0.0)) { - return {x_grid[ix], z_grid[iz]}; + const auto found = std::find(mask.begin(), mask.end(), static_cast(1U)); + if (found == mask.end()) { + return {0.0, 0.0}; + } + const auto index = static_cast(std::distance(mask.begin(), found)); + return {grid.x_grid[index % grid.x_grid.size()], grid.z_grid[index / grid.x_grid.size()]}; } - const auto centroid_row = clamp_index(static_cast(std::llround(row_weighted_sum / weight_sum)), height); - const auto centroid_col = clamp_index(static_cast(std::llround(col_weighted_sum / weight_sum)), width); - return {x_grid[centroid_col], z_grid[centroid_row]}; + return {x_weighted_sum / weight_sum, z_weighted_sum / weight_sum}; +} + +[[nodiscard]] auto compact_peak_centroid( + const std::vector& image, + const GridDefinition& grid, + std::size_t peak_index, + double peak +) -> std::tuple> { + const std::size_t width = grid.x_grid.size(); + const auto peak_row = peak_index / width; + const auto peak_col = peak_index % width; + const double x_peak = grid.x_grid[peak_col]; + const double z_peak = grid.z_grid[peak_row]; + const double threshold = kCenterThresholdFrac * peak; + + std::vector center_mask(image.size(), 0U); + for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) { + const double z_m = grid.z_grid[row]; + if (std::abs(z_m - z_peak) > kCenterRadiusZM) { + continue; + } + for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) { + const double x_m = grid.x_grid[col]; + const auto index = (row * width) + col; + if (std::abs(x_m - x_peak) <= kCenterRadiusXM && image[index] >= threshold) { + center_mask[index] = 1U; + } + } + } + + if (mask_count(center_mask) == 0U) { + center_mask[peak_index] = 1U; + } + + double weight_sum = 0.0; + double x_weighted_sum = 0.0; + double z_weighted_sum = 0.0; + for (std::size_t index = 0U; index < image.size(); ++index) { + if (center_mask[index] == 0U) { + continue; + } + double weight = std::pow(std::max(image[index] - threshold, 0.0), kCenterWeightPower); + if (!(weight > 0.0)) { + weight = image[index]; + } + if (!(weight > 0.0)) { + continue; + } + + const auto row = index / width; + const auto col = index % width; + weight_sum += weight; + x_weighted_sum += grid.x_grid[col] * weight; + z_weighted_sum += grid.z_grid[row] * weight; + } + + if (!(weight_sum > 0.0)) { + return {x_peak, z_peak, std::move(center_mask)}; + } + return {x_weighted_sum / weight_sum, z_weighted_sum / weight_sum, std::move(center_mask)}; +} + +[[nodiscard]] auto build_window_mask( + const GridDefinition& grid, + double x_center_m, + double z_center_m, + double radius_x_m, + double radius_z_m +) -> std::vector { + const std::size_t width = grid.x_grid.size(); + std::vector mask(width * grid.z_grid.size(), 0U); + for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) { + if (std::abs(grid.z_grid[row] - z_center_m) > radius_z_m) { + continue; + } + for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) { + if (std::abs(grid.x_grid[col] - x_center_m) <= radius_x_m) { + mask[(row * width) + col] = 1U; + } + } + } + return mask; +} + +[[nodiscard]] auto find_bp_objects( + const std::vector& bp_image, + const GridDefinition& grid +) -> std::vector { + std::vector objects{}; + if (bp_image.empty() || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) { + return objects; + } + + std::vector work = bp_image; + const double global_peak = max_value(work); + const double stop_level = kObjectMinFrac * global_peak; + if (!(global_peak > 0.0)) { + return objects; + } + + const std::size_t width = grid.x_grid.size(); + const std::size_t height = grid.z_grid.size(); + const double dx_cm = std::abs(grid.x_grid[1] - grid.x_grid[0]) * 100.0; + const double dz_cm = std::abs(grid.z_grid[1] - grid.z_grid[0]) * 100.0; + const double pixel_area_cm2 = dx_cm * dz_cm; + + for (std::size_t step = 0U; step < kMaxObjects; ++step) { + const auto peak_it = std::max_element(work.begin(), work.end()); + const double peak = *peak_it; + if (peak <= stop_level || !(peak > 0.0)) { + break; + } + + const auto peak_index = static_cast(std::distance(work.begin(), peak_it)); + const auto peak_row = peak_index / width; + const auto peak_col = peak_index % width; + const double x_peak = grid.x_grid[peak_col]; + const double z_peak = grid.z_grid[peak_row]; + + const double region_threshold = kRegionThresholdFrac * peak; + auto region_mask = component_containing_peak(work, width, height, peak_index, region_threshold); + const double region_area_cm2 = static_cast(mask_count(region_mask)) * pixel_area_cm2; + if (region_area_cm2 < kMinRegionAreaCm2) { + work[peak_index] = 0.0; + continue; + } + + const auto [x_region, z_region] = weighted_centroid(work, region_mask, grid, region_threshold); + auto [x_center, z_center, center_mask] = compact_peak_centroid(work, grid, peak_index, peak); + const double center_area_cm2 = static_cast(mask_count(center_mask)) * pixel_area_cm2; + + double sum_value = 0.0; + std::size_t value_count = 0U; + for (std::size_t index = 0U; index < work.size(); ++index) { + if (region_mask[index] == 0U) { + continue; + } + sum_value += work[index]; + value_count += 1U; + } + + ObjectRecord object{}; + object.index = objects.size() + 1U; + object.x_peak_m = x_peak; + object.z_peak_m = z_peak; + object.x_m = x_center; + object.z_m = z_center; + object.x_region_m = x_region; + object.z_region_m = z_region; + object.peak = peak; + object.area_cm2 = region_area_cm2; + object.center_area_cm2 = center_area_cm2; + object.mean_value = value_count > 0U ? sum_value / static_cast(value_count) : 0.0; + object.sum_value = sum_value; + object.region_mask = std::move(region_mask); + object.center_mask = std::move(center_mask); + objects.push_back(std::move(object)); + + const auto suppress_window = build_window_mask(grid, x_peak, z_peak, kSuppressRadiusXM, kSuppressRadiusZM); + auto suppress_mask = component_containing_peak( + work, + width, + height, + peak_index, + kSuppressThresholdFrac * peak, + &suppress_window + ); + if (mask_count(suppress_mask) < mask_count(objects.back().region_mask)) { + suppress_mask = objects.back().region_mask; + } + + for (std::size_t index = 0U; index < work.size(); ++index) { + if (suppress_mask[index] != 0U) { + work[index] = 0.0; + } + } + } + + return objects; +} + +[[nodiscard]] auto bistatic_depth_signature( + const ObjectRecord& object, + const std::vector& selected_traces, + const std::vector& x_tx, + const std::vector& x_rx +) -> std::vector { + std::vector signature{}; + signature.reserve(selected_traces.size()); + for (const auto& trace : selected_traces) { + const double r_tx = std::hypot(object.x_m - x_tx[trace.tx_local_index], object.z_m); + const double r_rx = std::hypot(object.x_m - x_rx[trace.rx_local_index], object.z_m); + signature.push_back(0.5 * (r_tx + r_rx)); + } + return signature; +} + +void mark_sidelobe_candidates( + std::vector& objects, + const std::vector& selected_traces, + const std::vector& x_tx, + const std::vector& x_rx +) { + std::vector> signatures{}; + signatures.reserve(objects.size()); + for (const auto& object : objects) { + signatures.push_back(bistatic_depth_signature(object, selected_traces, x_tx, x_rx)); + } + + for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) { + auto& object = objects[object_index]; + std::size_t best_parent = 0U; + double best_rms = std::numeric_limits::infinity(); + + for (std::size_t parent_index = 0U; parent_index < object_index; ++parent_index) { + const auto& parent = objects[parent_index]; + const double relative_peak = object.peak / (parent.peak + 1e-12); + const double dx_m = std::abs(object.x_m - parent.x_m); + const double dz_m = std::abs(object.z_m - parent.z_m); + + double squared_sum = 0.0; + const auto& object_signature = signatures[object_index]; + const auto& parent_signature = signatures[parent_index]; + for (std::size_t index = 0U; index < object_signature.size(); ++index) { + const double delta = object_signature[index] - parent_signature[index]; + squared_sum += delta * delta; + } + const double rms = object_signature.empty() + ? std::numeric_limits::infinity() + : std::sqrt(squared_sum / static_cast(object_signature.size())); + + const bool candidate = + relative_peak <= kSidelobeMaxRelativePeak && + dx_m >= kSidelobeMinDxM && + dz_m <= kSidelobeMaxDzM && + rms <= kSidelobeRangeRmsToleranceM; + + if (candidate && rms < best_rms) { + best_parent = parent.index; + best_rms = rms; + } + } + + if (best_parent != 0U) { + object.sidelobe_candidate = true; + object.sidelobe_parent = best_parent; + object.sidelobe_range_rms_m = best_rms; + } + } } [[nodiscard]] auto flatten_table( @@ -934,235 +1222,6 @@ void apply_motion_correction( return payload; } -[[nodiscard]] auto accumulator_max(const std::vector& values) -> double { - if (values.empty()) { - return 0.0; - } - return *std::max_element(values.begin(), values.end()); -} - -[[nodiscard]] auto clean_find_points( - const GridDefinition& grid, - const std::unordered_map>& peaks_by_pair, - const std::vector& x_tx, - const std::vector& x_rx, - double velocity_mps, - double shell_sigma_m, - PeakDomain domain -) -> std::pair, std::vector> { - std::vector found{}; - const auto accumulator = build_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx, domain); - const double initial_max = accumulator_max(accumulator); - if (!(initial_max > 0.0) || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) { - return {found, gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kGaussianSigma)}; - } - - const double dx = grid.x_grid[1] - grid.x_grid[0]; - const double dz = grid.z_grid[1] - grid.z_grid[0]; - const auto radius_x = static_cast(std::max(1.0, std::round(kCleanSuppressRadiusM / std::max(dx, 1e-6)))); - const auto radius_z = static_cast(std::max(1.0, std::round(kCleanSuppressRadiusM / std::max(dz, 1e-6)))); - - std::vector> excluded_ranges{}; - for (std::size_t step = 0U; step < kMaxObjects; ++step) { - const auto current = build_accumulator( - grid, - peaks_by_pair, - excluded_ranges, - velocity_mps, - shell_sigma_m, - x_tx, - x_rx, - domain - ); - const auto smoothed = gaussian_filter_2d(current, grid.x_grid.size(), grid.z_grid.size(), kGaussianSigma); - const double smoothed_max = accumulator_max(smoothed); - if (!(smoothed_max > (kCleanThresholdFrac * initial_max))) { - break; - } - - const auto max_it = std::max_element(smoothed.begin(), smoothed.end()); - const auto max_index = static_cast(std::distance(smoothed.begin(), max_it)); - const auto iz = max_index / grid.x_grid.size(); - const auto ix = max_index % grid.x_grid.size(); - const auto [x_est, z_est] = find_centroid( - smoothed, - grid.x_grid.size(), - grid.z_grid.size(), - iz, - ix, - radius_z, - radius_x, - grid.x_grid, - grid.z_grid - ); - - PointRecord point{}; - point.x_m = x_est; - point.z_m = z_est; - point.score = count_agreeing_ellipses( - x_est, - z_est, - peaks_by_pair, - excluded_ranges, - x_tx, - x_rx, - velocity_mps, - shell_sigma_m, - domain - ); - found.push_back(point); - - std::vector matched_depths{}; - for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) { - for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) { - const auto peak_it = peaks_by_pair.find(make_pair_key(static_cast(tx_index), static_cast(rx_index))); - if (peak_it == peaks_by_pair.end()) { - continue; - } - for (const auto& peak : peak_it->second) { - if (is_excluded(peak_depth_for_domain(peak, domain), excluded_ranges)) { - continue; - } - const double rt = std::sqrt(std::pow(x_est - x_tx[tx_index], 2.0) + std::pow(z_est, 2.0)); - const double rr = std::sqrt(std::pow(x_est - x_rx[rx_index], 2.0) + std::pow(z_est, 2.0)); - if (std::abs((rt + rr) - (velocity_mps * peak_tau_for_domain(peak, domain))) < shell_sigma_m * 3.0) { - matched_depths.push_back(peak_depth_for_domain(peak, domain)); - } - } - } - } - - if (!matched_depths.empty()) { - const auto [min_it, max_it_depth] = std::minmax_element(matched_depths.begin(), matched_depths.end()); - excluded_ranges.emplace_back(*min_it - shell_sigma_m, *max_it_depth + shell_sigma_m); - } - } - - return {found, gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kGaussianSigma)}; -} - -[[nodiscard]] auto extended_find_regions( - const GridDefinition& grid, - const std::unordered_map>& peaks_by_pair, - const std::vector& x_tx, - const std::vector& x_rx, - double velocity_mps, - double shell_sigma_m -) -> std::pair, std::vector> { - std::vector regions{}; - const auto accumulator = build_accumulator( - grid, - peaks_by_pair, - {}, - velocity_mps, - shell_sigma_m, - x_tx, - x_rx, - PeakDomain::Apparent - ); - const auto smoothed = gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kGaussianSigma); - const double smoothed_max = accumulator_max(smoothed); - if (!(smoothed_max > 0.0) || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) { - return {regions, smoothed}; - } - - const double dx_cm = (grid.x_grid[1] - grid.x_grid[0]) * 100.0; - const double dz_cm = (grid.z_grid[1] - grid.z_grid[0]) * 100.0; - const auto min_pixels = static_cast(std::max(1.0, std::floor(kExtendedMinAreaCm2 / std::max(dx_cm * dz_cm, 1e-6)))); - - const std::size_t width = grid.x_grid.size(); - const std::size_t height = grid.z_grid.size(); - const double threshold = kExtendedThresholdFrac * smoothed_max; - std::vector visited(width * height, 0U); - - for (std::size_t row = 0U; row < height; ++row) { - for (std::size_t col = 0U; col < width; ++col) { - const auto start_index = (row * width) + col; - if (visited[start_index] != 0U || smoothed[start_index] <= threshold) { - continue; - } - - std::vector stack{start_index}; - std::vector component{}; - visited[start_index] = 1U; - - while (!stack.empty()) { - const auto cell_index = stack.back(); - stack.pop_back(); - component.push_back(cell_index); - - const auto cell_row = cell_index / width; - const auto cell_col = cell_index % width; - const std::pair offsets[] = { - {-1, 0}, - {1, 0}, - {0, -1}, - {0, 1}, - }; - - for (const auto& [row_offset, col_offset] : offsets) { - const auto next_row = static_cast(cell_row) + row_offset; - const auto next_col = static_cast(cell_col) + col_offset; - if (next_row < 0 || next_col < 0) { - continue; - } - if (next_row >= static_cast(height) || next_col >= static_cast(width)) { - continue; - } - const auto next_index = - (static_cast(next_row) * width) + static_cast(next_col); - if (visited[next_index] != 0U || smoothed[next_index] <= threshold) { - continue; - } - visited[next_index] = 1U; - stack.push_back(next_index); - } - } - - if (component.size() < min_pixels) { - continue; - } - - RegionRecord region{}; - region.mask.assign(width * height, 0.0F); - double weight_sum = 0.0; - double x_weight_sum = 0.0; - double z_weight_sum = 0.0; - for (const auto cell_index : component) { - const auto cell_row = cell_index / width; - const auto cell_col = cell_index % width; - const double weight = smoothed[cell_index]; - weight_sum += weight; - x_weight_sum += grid.x_grid[cell_col] * weight; - z_weight_sum += grid.z_grid[cell_row] * weight; - region.mask[cell_index] = 1.0F; - } - - if (!(weight_sum > 0.0)) { - continue; - } - - region.x_m = x_weight_sum / weight_sum; - region.z_m = z_weight_sum / weight_sum; - region.score = count_agreeing_ellipses( - region.x_m, - region.z_m, - peaks_by_pair, - {}, - x_tx, - x_rx, - velocity_mps, - shell_sigma_m, - PeakDomain::Apparent - ); - region.pixel_count = static_cast(component.size()); - regions.push_back(std::move(region)); - } - } - - return {regions, smoothed}; -} - } // namespace auto GprProcessor::name() const -> std::string { @@ -1186,7 +1245,7 @@ auto GprProcessor::process_collection( 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(run_config, collection, selection, background_mean); + const auto selected_traces = collect_selected_traces(collection, selection, background_mean); if (selected_traces.empty()) { return results; } @@ -1195,160 +1254,87 @@ auto GprProcessor::process_collection( kSpeedOfLightMetersPerSec / std::sqrt(std::max(1e-6, static_cast(run_config.gpr.relative_permittivity))); const double start_hz = static_cast(live_config.gpr_start_freq_mhz) * 1'000'000.0; const double stop_hz = static_cast(live_config.gpr_stop_freq_mhz) * 1'000'000.0; - const double snr_thresh = std::max(0.0, static_cast(live_config.gpr_snr_thresh)); - const double snr_comp_max = std::max(0.0, static_cast(live_config.gpr_snr_comp_max)); + const double min_depth_m = static_cast(live_config.gpr_min_depth_m); + const double max_depth_m = static_cast(live_config.gpr_max_depth_m); + if (!(max_depth_m > min_depth_m) || !(max_depth_m > kGridZMinM)) { + return results; + } std::unordered_map ascans_by_pair{}; - double bandwidth_hz = 0.0; for (const auto& trace : selected_traces) { - const auto key = make_pair_key(trace.tx_local_index, trace.rx_local_index); - auto ascan = compute_ascan(trace, start_hz, stop_hz, velocity_mps); - if (ascan.amplitude.empty() || !(ascan.bandwidth_hz > 0.0)) { + auto ascan = compute_ascan(trace, start_hz, stop_hz); + if (ascan.samples.empty() || !(ascan.bandwidth_hz > 0.0) || !(ascan.dt_s > 0.0)) { continue; } - bandwidth_hz = std::max(bandwidth_hz, ascan.bandwidth_hz); - ascans_by_pair.emplace(key, std::move(ascan)); + ascans_by_pair.emplace( + make_pair_key(trace.tx_local_index, trace.rx_local_index), + std::move(ascan) + ); } - if (ascans_by_pair.empty() || !(bandwidth_hz > 0.0)) { + if (ascans_by_pair.empty()) { return results; } - const double max_depth_m = static_cast(live_config.gpr_max_depth_m); const auto grid = build_grid(selection.x_tx, selection.x_rx, max_depth_m); if (grid.x_grid.empty() || grid.z_grid.empty()) { return results; } - const double shell_sigma_m = velocity_mps / bandwidth_hz * 0.5; - std::unordered_map> peaks_by_pair{}; - for (std::size_t tx_index = 0U; tx_index < selection.x_tx.size(); ++tx_index) { - for (std::size_t rx_index = 0U; rx_index < selection.x_rx.size(); ++rx_index) { - const auto key = make_pair_key(static_cast(tx_index), static_cast(rx_index)); - const auto ascan_it = ascans_by_pair.find(key); - if (ascan_it == ascans_by_pair.end()) { - continue; - } - - const auto& ascan = ascan_it->second; - if (ascan.depth_m.size() < 3U || ascan.amplitude.size() < 3U) { - continue; - } - - const auto min_index = lower_bound_index(ascan.depth_m, static_cast(live_config.gpr_min_depth_m)); - const auto max_index = lower_bound_index(ascan.depth_m, static_cast(live_config.gpr_max_depth_m)); - if (max_index <= min_index + 2U) { - continue; - } - - const double noise = - median_copy(std::vector(ascan.amplitude.begin() + min_index, ascan.amplitude.begin() + max_index)); - - const double z_step = std::max(ascan.depth_m[1] - ascan.depth_m[0], 1e-6); - const std::size_t min_distance = static_cast(std::max( - 4.0, - std::floor(((velocity_mps / (2.0 * bandwidth_hz)) / z_step) * 0.7) - )); - const auto peak_indices = - find_peak_indices(ascan.amplitude, min_index, max_index, noise * snr_thresh, min_distance); - auto& peaks = peaks_by_pair[key]; - peaks.reserve(peak_indices.size()); - for (const auto peak_index : peak_indices) { - const double z_app = ascan.depth_m[peak_index]; - const double snr_raw = ascan.amplitude[peak_index] / std::max(noise, 1e-12); - const double attenuation = attenuation_at_depth(tx_index, rx_index, z_app, selection.x_tx, selection.x_rx); - const double attenuation_norm = - attenuation / attenuation_at_depth(tx_index, rx_index, 3.0, selection.x_tx, selection.x_rx); - const double snr_comp = std::min( - snr_raw / (std::pow(attenuation_norm, static_cast(live_config.gpr_comp_power)) + 1e-12), - snr_comp_max - ); - peaks.push_back(PeakRecord{ - .z_app = z_app, - .tau = ascan.time_s[peak_index], - .tau_corr = ascan.time_s[peak_index], - .z_corr = z_app, - .snr_raw = snr_raw, - .snr_comp = snr_comp, - }); - } - } - } - - if (peaks_by_pair.empty()) { - return results; - } - - if (run_config.gpr.mode == "extended") { - const auto [regions, smoothed_accumulator] = extended_find_regions( - grid, - peaks_by_pair, - selection.x_tx, - selection.x_rx, - velocity_mps, - shell_sigma_m - ); - results.collection_payloads.push_back( - build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator) - ); - - std::vector> region_rows{}; - region_rows.reserve(regions.size()); - for (std::size_t index = 0U; index < regions.size(); ++index) { - const auto& region = regions[index]; - region_rows.push_back( - { - static_cast(region.x_m), - static_cast(region.z_m), - static_cast(region.score), - static_cast(region.pixel_count), - } - ); - results.collection_payloads.push_back( - build_image_payload( - "gpr_region_mask_" + std::to_string(index), - grid.x_grid, - grid.z_grid, - std::vector(region.mask.begin(), region.mask.end()) - ) - ); - } - results.collection_payloads.push_back(build_table_payload("gpr_region_centers", region_rows, 4U)); - return results; - } - - const auto motion_timing_by_pair = build_motion_timing_by_pair( + auto bp = backproject_coherent( selected_traces, - run_config.run_combos.size(), - collection.capture_start_ns, - collection.capture_end_ns, - live_config, - velocity_mps - ); - apply_motion_correction(peaks_by_pair, motion_timing_by_pair, velocity_mps); - - const auto [points, smoothed_accumulator] = clean_find_points( + ascans_by_pair, grid, - peaks_by_pair, selection.x_tx, selection.x_rx, velocity_mps, - shell_sigma_m, - PeakDomain::Corrected + min_depth_m, + max_depth_m, + std::max(0.0, static_cast(live_config.gpr_range_comp_power)), + std::max(0.0, static_cast(live_config.gpr_angle_comp_power)) ); - results.collection_payloads.push_back(build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator)); + if (bp.image.empty()) { + return results; + } + + normalize_in_place(bp.image); + auto display_map = gaussian_filter_2d(bp.image, grid.x_grid.size(), grid.z_grid.size(), kSmoothSigma); + apply_depth_gate(display_map, grid.z_grid, grid.x_grid.size(), min_depth_m, max_depth_m); + normalize_in_place(display_map); + + auto objects = find_bp_objects(display_map, grid); + mark_sidelobe_candidates(objects, selected_traces, selection.x_tx, selection.x_rx); + + if (live_config.gpr_remove_sidelobe_objects_enabled) { + for (const auto& object : objects) { + if (!object.sidelobe_candidate) { + continue; + } + for (std::size_t index = 0U; index < display_map.size(); ++index) { + if (object.region_mask[index] != 0U) { + display_map[index] = 0.0; + } + } + } + } + + results.collection_payloads.push_back(build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, display_map)); std::vector> point_rows{}; - point_rows.reserve(points.size()); - for (const auto& point : points) { + point_rows.reserve(objects.size()); + for (const auto& object : objects) { + if (live_config.gpr_remove_sidelobe_objects_enabled && object.sidelobe_candidate) { + continue; + } point_rows.push_back( { - static_cast(point.x_m), - static_cast(point.z_m), - static_cast(point.score), + static_cast(object.x_m), + static_cast(object.z_m), + static_cast(object.peak), } ); } results.collection_payloads.push_back(build_table_payload("gpr_points", point_rows, 3U)); + return results; } diff --git a/docs/run_config.md b/docs/run_config.md index 0dc2855..0845fb9 100644 --- a/docs/run_config.md +++ b/docs/run_config.md @@ -236,7 +236,6 @@ GPR geometry and processing configuration. ```json "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ {"output_pos": 0, "x_m": 0.905} @@ -251,7 +250,6 @@ Fields: | Field | Meaning | | --- | --- | -| `mode` | GPR processing mode. | | `relative_permittivity` | Medium relative permittivity used for propagation speed. | | `tx_geometry` | Transmitter positions keyed by output switch position. | | `rx_geometry` | Receiver positions keyed by input switch position. | @@ -321,4 +319,3 @@ Compact-M K209 via remote server: "driver_mode": "native" } ``` - diff --git a/python_app/gui/app_window.py b/python_app/gui/app_window.py index 4b4092b..d8ddae4 100644 --- a/python_app/gui/app_window.py +++ b/python_app/gui/app_window.py @@ -214,6 +214,7 @@ class AppWindow( self._refresh_preprocess_summary_labels() self._apply_initial_radar_limits() self._start_locator_service() + self._on_processing_mode_changed(self._processing_mode.currentText()) self._write_live_processing_config() self._timer.start() 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 f36ce0b..e3a0316 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 @@ -42,15 +42,13 @@ class AppWindowLiveProcessingMixin: gpr_output_positions=self._parse_csv_int_list(self._gpr_output_positions_input.text()), gpr_min_depth_m=float(self._gpr_min_depth_m.value()), gpr_max_depth_m=float(self._gpr_max_depth_m.value()), - gpr_comp_power=float(self._gpr_comp_power.value()), + gpr_range_comp_power=float(self._gpr_range_comp_power.value()), + gpr_angle_comp_power=float(self._gpr_angle_comp_power.value()), gpr_start_freq_mhz=float(self._gpr_start_freq_mhz.value()), gpr_stop_freq_mhz=float(self._gpr_stop_freq_mhz.value()), - gpr_speed_m_s=float(self._gpr_speed_m_s.value()), - gpr_look_angle_deg=float(self._gpr_look_angle_deg.value()), - gpr_snr_thresh=float(self._gpr_snr_thresh.value()), - gpr_snr_comp_max=float(self._gpr_snr_comp_max.value()), gpr_background_subtract_enabled=bool(self._gpr_background_subtract_enabled.isChecked()), gpr_background_mean_count=int(self._gpr_background_mean_count.value()), + gpr_remove_sidelobe_objects_enabled=bool(self._gpr_remove_sidelobe_objects_enabled.isChecked()), history_command_seq=int(self._history_command_seq), history_command=str(history_command), ) @@ -61,25 +59,25 @@ class AppWindowLiveProcessingMixin: self._history_command_seq += 1 self._live_config_writer.write(self._live_processing_config(history_command=history_command)) - def _apply_external_gpr_speed_update(self, speed_m_s: float) -> None: - """Apply GPR speed received from locator clients without recursive signals.""" - with QSignalBlocker(self._gpr_speed_m_s): - self._gpr_speed_m_s.setValue(float(speed_m_s)) - self._write_live_processing_config() - def _on_processing_live_settings_changed(self, *_args) -> None: """Handle live-processing setting changes and trigger redraw when needed.""" try: - self._write_live_processing_config() current_mode = self._processing_mode.currentText() + if current_mode == "gpr": + self._drain_results_until_quiet(timeout_s=0.05, poll_s=0.005) + + self._write_live_processing_config() if current_mode == "bscan": self._drain_results_until_quiet(timeout_s=0.25, poll_s=0.01) self._sync_bscan_history_from_results() self._draw_bscan_heatmap_from_history() elif current_mode == "gpr": - self._drain_results_until_quiet(timeout_s=0.35, poll_s=0.01) - if self._result_history: - if not self._draw_results(self._result_history[-1]): + latest = self._drain_results_until_quiet(timeout_s=0.8, poll_s=0.02) + collection = latest + if collection is None and self._result_history: + collection = self._result_history[-1] + if collection is not None: + if not self._draw_results(collection): self._clear_gpr_plot() else: self._clear_gpr_plot() @@ -121,6 +119,19 @@ class AppWindowLiveProcessingMixin: except Exception as exc: # noqa: BLE001 self._show_exception("Failed to update locator GPR window", exc) + def _set_processing_mode_page(self, mode: str) -> None: + """Show the parameter page for `mode` without changing runtime state.""" + mode_to_page = { + "pass_through": 0, + "bscan": 1, + "gpr": 2, + } + self._processing_mode_pages.setCurrentIndex(mode_to_page.get(mode, 0)) + current_page = self._processing_mode_pages.currentWidget() + if current_page is not None: + self._processing_mode_pages.setFixedHeight(current_page.sizeHint().height()) + self._processing_mode_pages.updateGeometry() + def _on_processing_mode_changed(self, mode: str) -> None: """Switch processing parameter page and refresh corresponding visualization.""" previous_mode = getattr(self, "_active_processing_mode", "pass_through") @@ -137,21 +148,12 @@ class AppWindowLiveProcessingMixin: return self._active_processing_mode = mode - mode_to_page = { - "pass_through": 0, - "bscan": 1, - "gpr": 2, - } self._set_plot_mode(mode) - self._processing_mode_pages.setCurrentIndex(mode_to_page.get(mode, 0)) - current_page = self._processing_mode_pages.currentWidget() - if current_page is not None: - self._processing_mode_pages.setFixedHeight(current_page.sizeHint().height()) - self._processing_mode_pages.updateGeometry() + self._set_processing_mode_page(mode) self._on_processing_live_settings_changed() if mode == "gpr": self._publish_locator_snapshot_from_latest_result() - elif previous_mode == "gpr": + elif previous_mode == "gpr" and self._locator_service is not None: self._locator_service.publish_empty() if mode == "pass_through": self._log( @@ -178,14 +180,13 @@ class AppWindowLiveProcessingMixin: f"outputs={self._gpr_output_positions_input.text().strip() or ''}, " f"depth={self._gpr_min_depth_m.value():g}..{self._gpr_max_depth_m.value():g} m, " f"freq={self._gpr_start_freq_mhz.value():g}..{self._gpr_stop_freq_mhz.value():g} MHz, " - f"speed={self._gpr_speed_m_s.value():g} m/s, " - f"look_angle={self._gpr_look_angle_deg.value():g} deg, " - f"snr_thresh={self._gpr_snr_thresh.value():g}, " - f"snr_comp_max={self._gpr_snr_comp_max.value():g}, " + f"range_comp={self._gpr_range_comp_power.value():g}, " + f"angle_comp={self._gpr_angle_comp_power.value():g}, " f"background_subtract={self._gpr_background_subtract_enabled.isChecked()}, " f"mean_count={self._gpr_background_mean_count.value()}, " + f"remove_sidelobes={self._gpr_remove_sidelobe_objects_enabled.isChecked()}, " f"render_mode={self._gpr_render_mode.currentText()}, " - f"min_pairs={self._gpr_min_visible_pair_count.value()})" + f"min_score={self._gpr_min_visible_score.value():g})" ) def _clear_history_mode_caches(self) -> None: 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 aa9ff94..8dd5e61 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 @@ -194,7 +194,6 @@ class AppWindowConfigProfileIOMixin: self._bscan_start_freq_mhz, self._bscan_stop_freq_mhz, self._bscan_subtract_mean_ascan, - self._gpr_config_mode, self._gpr_relative_permittivity, self._gpr_tx_geometry_input, self._gpr_rx_geometry_input, @@ -202,17 +201,15 @@ class AppWindowConfigProfileIOMixin: self._gpr_output_positions_input, self._gpr_min_depth_m, self._gpr_max_depth_m, - self._gpr_comp_power, + self._gpr_range_comp_power, + self._gpr_angle_comp_power, self._gpr_start_freq_mhz, self._gpr_stop_freq_mhz, - self._gpr_speed_m_s, - self._gpr_look_angle_deg, - self._gpr_snr_thresh, - self._gpr_snr_comp_max, self._gpr_background_subtract_enabled, self._gpr_background_mean_count, + self._gpr_remove_sidelobe_objects_enabled, self._gpr_render_mode, - self._gpr_min_visible_pair_count, + self._gpr_min_visible_score, self._gpr_visible_x_min_m, self._gpr_visible_x_max_m, self._gpr_visible_z_min_m, @@ -253,7 +250,6 @@ class AppWindowConfigProfileIOMixin: self._bscan_stop_freq_mhz.setValue(float(gui_state.processing.bscan.stop_freq_mhz)) self._bscan_subtract_mean_ascan.setChecked(bool(gui_state.processing.bscan.subtract_mean_ascan)) - self._set_combo_current_text(self._gpr_config_mode, str(config.gpr.mode)) self._gpr_relative_permittivity.setValue(float(config.gpr.relative_permittivity)) self._gpr_tx_geometry_input.setPlainText( "\n".join( @@ -271,19 +267,19 @@ class AppWindowConfigProfileIOMixin: self._gpr_output_positions_input.setText(str(gui_state.processing.gpr.output_positions)) self._gpr_min_depth_m.setValue(float(gui_state.processing.gpr.min_depth_m)) self._gpr_max_depth_m.setValue(float(gui_state.processing.gpr.max_depth_m)) - self._gpr_comp_power.setValue(float(gui_state.processing.gpr.comp_power)) + 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._gpr_start_freq_mhz.setValue(float(gui_state.processing.gpr.start_freq_mhz)) self._gpr_stop_freq_mhz.setValue(float(gui_state.processing.gpr.stop_freq_mhz)) - self._gpr_speed_m_s.setValue(float(gui_state.processing.gpr.speed_m_s)) - self._gpr_look_angle_deg.setValue(float(gui_state.processing.gpr.look_angle_deg)) - self._gpr_snr_thresh.setValue(float(gui_state.processing.gpr.snr_thresh)) - self._gpr_snr_comp_max.setValue(float(gui_state.processing.gpr.snr_comp_max)) self._gpr_background_subtract_enabled.setChecked( bool(gui_state.processing.gpr.background_subtract_enabled) ) self._gpr_background_mean_count.setValue(int(gui_state.processing.gpr.background_mean_count)) + self._gpr_remove_sidelobe_objects_enabled.setChecked( + bool(gui_state.processing.gpr.remove_sidelobe_objects_enabled) + ) self._set_combo_current_text(self._gpr_render_mode, gui_state.processing.gpr.render_mode) - self._gpr_min_visible_pair_count.setValue(int(gui_state.processing.gpr.min_visible_pair_count)) + self._gpr_min_visible_score.setValue(float(gui_state.processing.gpr.min_visible_score)) self._gpr_visible_x_min_m.setValue(float(gui_state.processing.gpr.visible_x_min_m)) self._gpr_visible_x_max_m.setValue(float(gui_state.processing.gpr.visible_x_max_m)) self._gpr_visible_z_min_m.setValue(float(gui_state.processing.gpr.visible_z_min_m)) 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 1bc6437..027cce4 100644 --- a/python_app/gui/controllers/app_window_config/state_builders.py +++ b/python_app/gui/controllers/app_window_config/state_builders.py @@ -169,17 +169,15 @@ class AppWindowConfigStateBuildersMixin: output_positions=self._default_gpr_output_positions_from_config(config), min_depth_m=2.0, max_depth_m=14.0, - comp_power=0.2, + range_comp_power=0.28, + angle_comp_power=0.10, start_freq_mhz=3000.0, stop_freq_mhz=6000.0, - speed_m_s=0.0, - look_angle_deg=0.0, - snr_thresh=4.5, - snr_comp_max=25.0, background_subtract_enabled=True, background_mean_count=10, + remove_sidelobe_objects_enabled=True, render_mode="heatmap", - min_visible_pair_count=1, + min_visible_score=0.0, visible_x_min_m=default_gpr_x_min_m, visible_x_max_m=default_gpr_x_max_m, visible_z_min_m=0.0, @@ -258,17 +256,15 @@ class AppWindowConfigStateBuildersMixin: output_positions=self._gpr_output_positions_input.text().strip(), min_depth_m=float(self._gpr_min_depth_m.value()), max_depth_m=float(self._gpr_max_depth_m.value()), - comp_power=float(self._gpr_comp_power.value()), + range_comp_power=float(self._gpr_range_comp_power.value()), + angle_comp_power=float(self._gpr_angle_comp_power.value()), start_freq_mhz=float(self._gpr_start_freq_mhz.value()), stop_freq_mhz=float(self._gpr_stop_freq_mhz.value()), - speed_m_s=float(self._gpr_speed_m_s.value()), - look_angle_deg=float(self._gpr_look_angle_deg.value()), - snr_thresh=float(self._gpr_snr_thresh.value()), - snr_comp_max=float(self._gpr_snr_comp_max.value()), background_subtract_enabled=bool(self._gpr_background_subtract_enabled.isChecked()), background_mean_count=int(self._gpr_background_mean_count.value()), + remove_sidelobe_objects_enabled=bool(self._gpr_remove_sidelobe_objects_enabled.isChecked()), render_mode=self._gpr_render_mode.currentText(), - min_visible_pair_count=int(self._gpr_min_visible_pair_count.value()), + min_visible_score=float(self._gpr_min_visible_score.value()), visible_x_min_m=float(self._gpr_visible_x_min_m.value()), visible_x_max_m=float(self._gpr_visible_x_max_m.value()), visible_z_min_m=float(self._gpr_visible_z_min_m.value()), @@ -323,14 +319,13 @@ class AppWindowConfigStateBuildersMixin: combo_text = self._combos_text.text() config.combos = parse_combos_from_text(combo_text) config.ensure_combos() - if self._switches_are_effectively_static(config): + if self._processing_mode.currentText() != "gpr" and self._switches_are_effectively_static(config): config.combos = [ComboModel(input=0, output=0)] for key in PREPROCESS_ASSET_KEYS: preprocess_asset_model(config, key).bundle_path = "" for key in VISIBLE_PREPROCESS_ASSET_KEYS: preprocess_asset_model(config, key).set_name = self._selected_preprocess_sets.get(key, "") - config.gpr.mode = self._gpr_config_mode.currentText() config.gpr.relative_permittivity = float(self._gpr_relative_permittivity.value()) config.gpr.tx_geometry = self._parse_gpr_tx_geometry_text(self._gpr_tx_geometry_input.toPlainText()) config.gpr.rx_geometry = self._parse_gpr_rx_geometry_text(self._gpr_rx_geometry_input.toPlainText()) @@ -378,7 +373,7 @@ class AppWindowConfigStateBuildersMixin: @staticmethod def _switches_are_effectively_static(config: RunConfigModel) -> bool: - """Return `True` when switch setup effectively yields one fixed combo.""" + """Return `True` when non-GPR switch setup effectively yields one fixed combo.""" if config.is_multi_device: return False has_single_position = config.input_switch.positions <= 1 and config.output_switch.positions <= 1 diff --git a/python_app/gui/controllers/app_window_pipeline_mixin.py b/python_app/gui/controllers/app_window_pipeline_mixin.py index 089516a..3c12db9 100644 --- a/python_app/gui/controllers/app_window_pipeline_mixin.py +++ b/python_app/gui/controllers/app_window_pipeline_mixin.py @@ -399,21 +399,25 @@ class AppWindowPipelineMixin: previous = current time.sleep(poll_s) - def _drain_results_until_quiet(self, *, timeout_s: float, poll_s: float) -> None: - """Drain only results ring until size stabilizes or timeout expires.""" + def _drain_results_until_quiet(self, *, timeout_s: float, poll_s: float) -> ResultCollection | None: + """Drain results until at least one result arrives and the ring becomes quiet.""" if self._result_reader is None: - return + return None deadline = time.monotonic() + timeout_s stable_rounds = 0 + latest_seen: ResultCollection | None = None - while time.monotonic() < deadline and stable_rounds < 2: + while time.monotonic() < deadline and (latest_seen is None or stable_rounds < 2): latest = self._read_all_results() if latest is None: - stable_rounds += 1 + if latest_seen is not None: + stable_rounds += 1 else: + latest_seen = latest stable_rounds = 0 time.sleep(poll_s) + return latest_seen def _update_history_indicator(self) -> None: """Update UI label with current history buffer sizes.""" @@ -466,22 +470,25 @@ class AppWindowPipelineMixin: ) def _drain_locator_speed_updates(self) -> None: - """Apply queued speed updates received by the embedded locator server.""" - latest_speed = self._locator_service.drain_speed_updates() - if latest_speed is None: + """Drain queued locator speed packets; coherent BP does not use motion speed.""" + if self._locator_service is None: return - self._apply_external_gpr_speed_update(0.0) # TODO: remove temporary stub and apply real locator client speed. + self._locator_service.drain_speed_updates() def _publish_locator_snapshot_from_collection(self, collection: ResultCollection) -> None: """Publish one locator snapshot from a GPR result collection.""" + if self._locator_service is None: + return self._locator_service.publish_collection( collection, - float(self._gpr_min_visible_pair_count.value()), + float(self._gpr_min_visible_score.value()), visible_bounds=self._gpr_visible_object_bounds(), ) def _publish_locator_snapshot_from_latest_result(self) -> None: """Publish current locator-visible snapshot from latest cached GPR result.""" + if self._locator_service is None: + return if self._processing_mode.currentText() != "gpr": self._locator_service.publish_empty() return diff --git a/python_app/gui/controllers/app_window_plot/gpr_plot_mixin.py b/python_app/gui/controllers/app_window_plot/gpr_plot_mixin.py index 311b3d9..198d151 100644 --- a/python_app/gui/controllers/app_window_plot/gpr_plot_mixin.py +++ b/python_app/gui/controllers/app_window_plot/gpr_plot_mixin.py @@ -39,7 +39,7 @@ class AppWindowGprPlotMixin: if self._gpr_region_centers_item is not None: self._gpr_region_centers_item.setData(x=[], y=[]) self._gpr_region_centers_item.hide() - self._gpr_plot.setTitle(f"GPR {self._gpr_config_mode.currentText()}") + self._gpr_plot.setTitle("GPR coherent BP") def _configure_gpr_plot_axes(self) -> None: """Apply persistent GPR plot axis labels and base view settings.""" @@ -53,6 +53,29 @@ class AppWindowGprPlotMixin: view_box = plot.getViewBox() view_box.invertY(False) view_box.enableAutoRange(x=False, y=False) + self._disable_gpr_plot_interaction(plot, view_box) + + @staticmethod + def _disable_gpr_plot_interaction(plot: pg.PlotWidget, view_box: pg.ViewBox) -> None: + """Disable mouse-driven GPR pan/zoom; ranges are controlled by widgets.""" + AppWindowGprPlotMixin._call_if_present(plot, "setMouseEnabled", x=False, y=False) + AppWindowGprPlotMixin._call_if_present(plot, "setMenuEnabled", False) + AppWindowGprPlotMixin._call_if_present(plot.getPlotItem(), "hideButtons") + AppWindowGprPlotMixin._call_if_present(view_box, "setMouseEnabled", x=False, y=False) + AppWindowGprPlotMixin._call_if_present(view_box, "setMenuEnabled", False) + for axis_name in ("bottom", "left"): + AppWindowGprPlotMixin._call_if_present( + plot.getPlotItem().getAxis(axis_name), + "setMouseEnabled", + False, + ) + + @staticmethod + def _call_if_present(obj: object, method_name: str, *args, **kwargs) -> None: + """Call optional pyqtgraph API when available in the installed version.""" + method = getattr(obj, method_name, None) + if method is not None: + method(*args, **kwargs) def _ensure_gpr_plot_items(self) -> None: """Create persistent GPR plot items once and reuse them on redraw.""" @@ -209,8 +232,9 @@ class AppWindowGprPlotMixin: self._gpr_image_item.setLevels((float(np.min(image)), float(np.max(image) + 1e-6))) self._gpr_image_item.show() - plot.setXRange(x_min, x_max, padding=0.02) - plot.setYRange(self._gpr_display_y_min(y_min, y_max), y_max, padding=0.02) + visible_x_min, visible_x_max, visible_z_min, visible_z_max = self._gpr_visible_bounds() + plot.setXRange(visible_x_min, visible_x_max, padding=0.0) + plot.setYRange(self._gpr_display_y_min(visible_z_min, visible_z_max), visible_z_max, padding=0.0) self._draw_gpr_geometry_markers() @@ -227,7 +251,7 @@ class AppWindowGprPlotMixin: ) self._gpr_points_item.show() for x_value, y_value, score in points: - label = pg.TextItem(text=f"{float(score):.0f}", color="#ffffff", anchor=(0.0, 1.0)) + label = pg.TextItem(text=f"{float(score):.2f}", color="#ffffff", anchor=(0.0, 1.0)) label.setZValue(40) label.setPos(float(x_value), float(y_value)) plot.addItem(label) @@ -273,7 +297,7 @@ class AppWindowGprPlotMixin: self._gpr_region_mask_items.append(mask_image) self._gpr_region_contours.append(contour) - plot.setTitle(f"GPR {self._gpr_config_mode.currentText()}") + plot.setTitle("GPR coherent BP") finally: plot.setUpdatesEnabled(True) return True @@ -289,8 +313,8 @@ class AppWindowGprPlotMixin: half_span = 0.5 * minimum_span return center - half_span, center + half_span - def _gpr_visible_object_bounds(self) -> tuple[float, float, float, float]: - """Return normalized object-only visible X/Z bounds from GUI controls.""" + def _gpr_visible_bounds(self) -> tuple[float, float, float, float]: + """Return normalized GPR visible X/Z bounds from GUI controls.""" x_min, x_max = self._normalized_display_range( float(self._gpr_visible_x_min_m.value()), float(self._gpr_visible_x_max_m.value()), @@ -303,11 +327,17 @@ class AppWindowGprPlotMixin: ) return x_min, x_max, z_min, z_max + def _gpr_visible_object_bounds(self) -> tuple[float, float, float, float]: + """Return normalized object/locator visible X/Z bounds from GUI controls.""" + return self._gpr_visible_bounds() + @staticmethod def _gpr_display_y_min(z_min: float, z_max: float) -> float: - """Return lower GPR display bound with a small negative margin for antenna markers.""" - lower = min(0.0, float(z_min)) - span = max(float(z_max) - float(z_min), 1e-6) + """Return lower display bound, preserving surface markers only when surface is visible.""" + lower = float(z_min) + if lower > 0.0: + return lower + span = max(float(z_max) - lower, 1e-6) marker_margin = max(span * 0.03, 0.06) return lower - marker_margin @@ -343,9 +373,9 @@ class AppWindowGprPlotMixin: self._gpr_rx_item.hide() @staticmethod - def _format_gpr_object_label(x_m: float, z_m: float, pair_count: float) -> str: - """Format object-only annotation text with pair count and coordinates.""" - return f"{int(round(pair_count))} | x={x_m:.1f} | z={z_m:.1f}" + def _format_gpr_object_label(x_m: float, z_m: float, score: float) -> str: + """Format object-only annotation text with normalized BP score and coordinates.""" + return f"{score:.2f} | x={x_m:.1f} | z={z_m:.1f}" @staticmethod def _expanded_scene_rect(rect: QRectF, *, padding_px: float = 4.0) -> QRectF: @@ -411,7 +441,7 @@ class AppWindowGprPlotMixin: occupied_scene_rects.append(last_rect) def _gpr_object_rows(self, collection: ResultCollection) -> np.ndarray: - """Return object rows as `[x_m, z_m, pair_count]` from current GPR result payload.""" + """Return object rows as `[x_m, z_m, score]` from current GPR result payload.""" return extract_gpr_object_rows(collection) def _filtered_gpr_object_rows(self, collection: ResultCollection) -> np.ndarray: @@ -421,11 +451,11 @@ class AppWindowGprPlotMixin: return rows x_min, x_max, z_min, z_max = self._gpr_visible_object_bounds() - min_pair_count = float(self._gpr_min_visible_pair_count.value()) + min_score = float(self._gpr_min_visible_score.value()) finite_mask = np.all(np.isfinite(rows[:, :3]), axis=1) visible_mask = ( finite_mask - & (rows[:, 2] >= min_pair_count) + & (rows[:, 2] >= min_score) & (rows[:, 0] >= x_min) & (rows[:, 0] <= x_max) & (rows[:, 1] >= z_min) @@ -470,12 +500,12 @@ class AppWindowGprPlotMixin: occupied_scene_rects: list[QRectF] = [] x_span = x_max - x_min z_span = z_max - z_min - for x_value, z_value, pair_count in object_rows: + for x_value, z_value, score in object_rows: label = pg.TextItem( text=self._format_gpr_object_label( float(x_value), float(z_value), - float(pair_count), + float(score), ), color="#ffd6d9", anchor=(0.0, 1.0), @@ -497,7 +527,7 @@ class AppWindowGprPlotMixin: plot.setXRange(x_min, x_max, padding=0.0) plot.setYRange(self._gpr_display_y_min(z_min, z_max), z_max, padding=0.0) - plot.setTitle(f"GPR {self._gpr_config_mode.currentText()} Objects Only") + plot.setTitle("GPR coherent BP Objects Only") finally: plot.setUpdatesEnabled(True) return True diff --git a/python_app/gui/controllers/sections/processing_section.py b/python_app/gui/controllers/sections/processing_section.py index db895c1..5f5c3fd 100644 --- a/python_app/gui/controllers/sections/processing_section.py +++ b/python_app/gui/controllers/sections/processing_section.py @@ -154,10 +154,6 @@ def build_processing_group(owner) -> QGroupBox: gpr_defaults = owner._defaults_config.gpr - owner._gpr_config_mode = QComboBox() - owner._gpr_config_mode.addItems(["point", "extended"]) - owner._set_combo_current_text(owner._gpr_config_mode, gpr_defaults.mode) - owner._gpr_relative_permittivity = QDoubleSpinBox() owner._gpr_relative_permittivity.setDecimals(4) owner._gpr_relative_permittivity.setRange(0.0001, 1000.0) @@ -190,11 +186,17 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_max_depth_m.setSingleStep(0.1) owner._gpr_max_depth_m.setValue(float(gpr_live_defaults.max_depth_m)) - owner._gpr_comp_power = QDoubleSpinBox() - owner._gpr_comp_power.setDecimals(3) - owner._gpr_comp_power.setRange(0.0, 5.0) - owner._gpr_comp_power.setSingleStep(0.05) - owner._gpr_comp_power.setValue(float(gpr_live_defaults.comp_power)) + owner._gpr_range_comp_power = QDoubleSpinBox() + owner._gpr_range_comp_power.setDecimals(3) + owner._gpr_range_comp_power.setRange(0.0, 5.0) + owner._gpr_range_comp_power.setSingleStep(0.01) + owner._gpr_range_comp_power.setValue(float(gpr_live_defaults.range_comp_power)) + + owner._gpr_angle_comp_power = QDoubleSpinBox() + owner._gpr_angle_comp_power.setDecimals(3) + owner._gpr_angle_comp_power.setRange(0.0, 5.0) + owner._gpr_angle_comp_power.setSingleStep(0.01) + owner._gpr_angle_comp_power.setValue(float(gpr_live_defaults.angle_comp_power)) owner._gpr_start_freq_mhz = QDoubleSpinBox() owner._gpr_start_freq_mhz.setDecimals(1) @@ -208,30 +210,6 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_stop_freq_mhz.setSingleStep(10.0) owner._gpr_stop_freq_mhz.setValue(float(gpr_live_defaults.stop_freq_mhz)) - 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.01) - owner._gpr_speed_m_s.setValue(float(gpr_live_defaults.speed_m_s)) - - 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.1) - owner._gpr_look_angle_deg.setValue(float(gpr_live_defaults.look_angle_deg)) - - owner._gpr_snr_thresh = QDoubleSpinBox() - owner._gpr_snr_thresh.setDecimals(2) - owner._gpr_snr_thresh.setRange(0.0, 1_000.0) - owner._gpr_snr_thresh.setSingleStep(0.1) - owner._gpr_snr_thresh.setValue(float(gpr_live_defaults.snr_thresh)) - - owner._gpr_snr_comp_max = QDoubleSpinBox() - owner._gpr_snr_comp_max.setDecimals(2) - owner._gpr_snr_comp_max.setRange(0.0, 1_000.0) - owner._gpr_snr_comp_max.setSingleStep(0.5) - owner._gpr_snr_comp_max.setValue(float(gpr_live_defaults.snr_comp_max)) - owner._gpr_background_subtract_enabled = QCheckBox("Subtract mean of previous collections") owner._gpr_background_subtract_enabled.setChecked(bool(gpr_live_defaults.background_subtract_enabled)) @@ -239,13 +217,18 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_background_mean_count.setRange(0, 10_000) owner._gpr_background_mean_count.setValue(int(gpr_live_defaults.background_mean_count)) + owner._gpr_remove_sidelobe_objects_enabled = QCheckBox("Remove sidelobe objects") + owner._gpr_remove_sidelobe_objects_enabled.setChecked(bool(gpr_live_defaults.remove_sidelobe_objects_enabled)) + owner._gpr_render_mode = QComboBox() owner._gpr_render_mode.addItems(["heatmap", "objects_only"]) owner._set_combo_current_text(owner._gpr_render_mode, gpr_live_defaults.render_mode) - owner._gpr_min_visible_pair_count = QSpinBox() - owner._gpr_min_visible_pair_count.setRange(1, 10_000) - owner._gpr_min_visible_pair_count.setValue(int(gpr_live_defaults.min_visible_pair_count)) + owner._gpr_min_visible_score = QDoubleSpinBox() + owner._gpr_min_visible_score.setDecimals(2) + owner._gpr_min_visible_score.setRange(0.0, 1.0) + owner._gpr_min_visible_score.setSingleStep(0.05) + owner._gpr_min_visible_score.setValue(float(gpr_live_defaults.min_visible_score)) owner._gpr_visible_x_min_m = QDoubleSpinBox() owner._gpr_visible_x_min_m.setDecimals(2) @@ -274,31 +257,28 @@ def build_processing_group(owner) -> QGroupBox: gpr_page = _build_processing_mode_page( owner._processing_mode_pages, [ - ("Config mode", owner._gpr_config_mode), ("Relative permittivity", owner._gpr_relative_permittivity), ("Input positions", owner._gpr_input_positions_input), ("Output positions", owner._gpr_output_positions_input), ("Min depth m", owner._gpr_min_depth_m), ("Max depth m", owner._gpr_max_depth_m), - ("Comp power", owner._gpr_comp_power), - ("SNR thresh", owner._gpr_snr_thresh), - ("SNR comp max", owner._gpr_snr_comp_max), - ("Speed m/s", owner._gpr_speed_m_s), + ("Range comp power", owner._gpr_range_comp_power), + ("Angle comp power", owner._gpr_angle_comp_power), ("Render mode", owner._gpr_render_mode), - ("Min visible pairs", owner._gpr_min_visible_pair_count), + ("Min visible score", owner._gpr_min_visible_score), ("Tx geometry", owner._gpr_tx_geometry_input), ("Rx geometry", owner._gpr_rx_geometry_input), ("Start MHz", owner._gpr_start_freq_mhz), ("Stop MHz", owner._gpr_stop_freq_mhz), - ("Look angle deg", owner._gpr_look_angle_deg), ("Visible X min m", owner._gpr_visible_x_min_m), ("Visible X max m", owner._gpr_visible_x_max_m), ("Visible Z min m", owner._gpr_visible_z_min_m), ("Visible Z max m", owner._gpr_visible_z_max_m), owner._gpr_background_subtract_enabled, ("Mean count", owner._gpr_background_mean_count), + owner._gpr_remove_sidelobe_objects_enabled, ], - split_index=11, + split_index=10, ) owner._processing_mode_pages.addWidget(gpr_page) @@ -323,21 +303,19 @@ def build_processing_group(owner) -> QGroupBox: owner._gpr_output_positions_input.editingFinished.connect(owner._on_processing_live_settings_changed) owner._gpr_min_depth_m.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_max_depth_m.valueChanged.connect(owner._on_processing_live_settings_changed) - owner._gpr_comp_power.valueChanged.connect(owner._on_processing_live_settings_changed) + 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_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_speed_m_s.valueChanged.connect(owner._on_processing_live_settings_changed) - owner._gpr_look_angle_deg.valueChanged.connect(owner._on_processing_live_settings_changed) - owner._gpr_snr_thresh.valueChanged.connect(owner._on_processing_live_settings_changed) - owner._gpr_snr_comp_max.valueChanged.connect(owner._on_processing_live_settings_changed) owner._gpr_background_subtract_enabled.toggled.connect(owner._on_processing_live_settings_changed) owner._gpr_background_mean_count.valueChanged.connect(owner._on_processing_live_settings_changed) + owner._gpr_remove_sidelobe_objects_enabled.toggled.connect(owner._on_processing_live_settings_changed) owner._gpr_render_mode.currentTextChanged.connect(owner._on_gpr_visual_settings_changed) - owner._gpr_min_visible_pair_count.valueChanged.connect(owner._on_gpr_locator_threshold_changed) + owner._gpr_min_visible_score.valueChanged.connect(owner._on_gpr_locator_threshold_changed) owner._gpr_visible_x_min_m.valueChanged.connect(owner._on_gpr_locator_window_changed) owner._gpr_visible_x_max_m.valueChanged.connect(owner._on_gpr_locator_window_changed) owner._gpr_visible_z_min_m.valueChanged.connect(owner._on_gpr_locator_window_changed) owner._gpr_visible_z_max_m.valueChanged.connect(owner._on_gpr_locator_window_changed) - owner._on_processing_mode_changed(owner._processing_mode.currentText()) + owner._set_processing_mode_page(owner._processing_mode.currentText()) return group diff --git a/python_app/models/gui_profile_codec.py b/python_app/models/gui_profile_codec.py index 3fc6dfb..9551f69 100644 --- a/python_app/models/gui_profile_codec.py +++ b/python_app/models/gui_profile_codec.py @@ -184,10 +184,16 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: gui.processing.gpr.max_depth_m, "gui.processing.gpr", ), - comp_power=_optional_float( + range_comp_power=_optional_float( gpr_object, - "comp_power", - gui.processing.gpr.comp_power, + "range_comp_power", + gui.processing.gpr.range_comp_power, + "gui.processing.gpr", + ), + angle_comp_power=_optional_float( + gpr_object, + "angle_comp_power", + gui.processing.gpr.angle_comp_power, "gui.processing.gpr", ), start_freq_mhz=_optional_float( @@ -202,30 +208,6 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: gui.processing.gpr.stop_freq_mhz, "gui.processing.gpr", ), - speed_m_s=_optional_float( - gpr_object, - "speed_m_s", - gui.processing.gpr.speed_m_s, - "gui.processing.gpr", - ), - look_angle_deg=_optional_float( - gpr_object, - "look_angle_deg", - gui.processing.gpr.look_angle_deg, - "gui.processing.gpr", - ), - snr_thresh=_optional_float( - gpr_object, - "snr_thresh", - gui.processing.gpr.snr_thresh, - "gui.processing.gpr", - ), - snr_comp_max=_optional_float( - gpr_object, - "snr_comp_max", - gui.processing.gpr.snr_comp_max, - "gui.processing.gpr", - ), background_subtract_enabled=_optional_bool( gpr_object, "background_subtract_enabled", @@ -238,16 +220,22 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: gui.processing.gpr.background_mean_count, "gui.processing.gpr", ), + remove_sidelobe_objects_enabled=_optional_bool( + gpr_object, + "remove_sidelobe_objects_enabled", + gui.processing.gpr.remove_sidelobe_objects_enabled, + "gui.processing.gpr", + ), render_mode=_optional_string( gpr_object, "render_mode", gui.processing.gpr.render_mode, "gui.processing.gpr", ), - min_visible_pair_count=_optional_int( + min_visible_score=_optional_float( gpr_object, - "min_visible_pair_count", - gui.processing.gpr.min_visible_pair_count, + "min_visible_score", + gui.processing.gpr.min_visible_score, "gui.processing.gpr", ), visible_x_min_m=_optional_float( @@ -282,12 +270,12 @@ def gui_profile_from_dict(payload: dict[str, Any]) -> GuiProfileModel: raise ValueError("gui.processing.bscan.axis must be one of: abs, real, phase") if gui.processing.gpr.render_mode not in {"heatmap", "objects_only"}: raise ValueError("gui.processing.gpr.render_mode must be one of: heatmap, objects_only") - if gui.processing.gpr.snr_thresh < 0.0: - raise ValueError("gui.processing.gpr.snr_thresh must be >= 0") - if gui.processing.gpr.snr_comp_max < 0.0: - raise ValueError("gui.processing.gpr.snr_comp_max must be >= 0") - if gui.processing.gpr.min_visible_pair_count < 1: - raise ValueError("gui.processing.gpr.min_visible_pair_count must be >= 1") + if gui.processing.gpr.range_comp_power < 0.0: + raise ValueError("gui.processing.gpr.range_comp_power must be >= 0") + if gui.processing.gpr.angle_comp_power < 0.0: + raise ValueError("gui.processing.gpr.angle_comp_power must be >= 0") + if gui.processing.gpr.min_visible_score < 0.0: + raise ValueError("gui.processing.gpr.min_visible_score must be >= 0") data_actions_object = _as_dict(gui_object.get("data_actions"), "gui.data_actions") gui.data_actions = GuiDataActionsStateModel( @@ -372,17 +360,15 @@ def gui_profile_to_dict(model: GuiProfileModel) -> dict[str, Any]: "output_positions": gui.processing.gpr.output_positions, "min_depth_m": gui.processing.gpr.min_depth_m, "max_depth_m": gui.processing.gpr.max_depth_m, - "comp_power": gui.processing.gpr.comp_power, + "range_comp_power": gui.processing.gpr.range_comp_power, + "angle_comp_power": gui.processing.gpr.angle_comp_power, "start_freq_mhz": gui.processing.gpr.start_freq_mhz, "stop_freq_mhz": gui.processing.gpr.stop_freq_mhz, - "speed_m_s": gui.processing.gpr.speed_m_s, - "look_angle_deg": gui.processing.gpr.look_angle_deg, - "snr_thresh": gui.processing.gpr.snr_thresh, - "snr_comp_max": gui.processing.gpr.snr_comp_max, "background_subtract_enabled": gui.processing.gpr.background_subtract_enabled, "background_mean_count": gui.processing.gpr.background_mean_count, + "remove_sidelobe_objects_enabled": gui.processing.gpr.remove_sidelobe_objects_enabled, "render_mode": gui.processing.gpr.render_mode, - "min_visible_pair_count": gui.processing.gpr.min_visible_pair_count, + "min_visible_score": gui.processing.gpr.min_visible_score, "visible_x_min_m": gui.processing.gpr.visible_x_min_m, "visible_x_max_m": gui.processing.gpr.visible_x_max_m, "visible_z_min_m": gui.processing.gpr.visible_z_min_m, diff --git a/python_app/models/gui_profile_schema.py b/python_app/models/gui_profile_schema.py index 42dbc81..e0fd079 100644 --- a/python_app/models/gui_profile_schema.py +++ b/python_app/models/gui_profile_schema.py @@ -53,17 +53,15 @@ class GuiGprStateModel: output_positions: str = "" min_depth_m: float = 2.0 max_depth_m: float = 14.0 - comp_power: float = 0.2 + range_comp_power: float = 0.28 + angle_comp_power: float = 0.10 start_freq_mhz: float = 3000.0 stop_freq_mhz: float = 6000.0 - speed_m_s: float = 0.0 - look_angle_deg: float = 0.0 - snr_thresh: float = 4.5 - snr_comp_max: float = 25.0 background_subtract_enabled: bool = True background_mean_count: int = 10 + remove_sidelobe_objects_enabled: bool = True render_mode: str = "heatmap" - min_visible_pair_count: int = 1 + min_visible_score: float = 0.0 visible_x_min_m: float = -2.0 visible_x_max_m: float = 2.0 visible_z_min_m: float = 0.0 diff --git a/python_app/models/run_config_codec.py b/python_app/models/run_config_codec.py index 641526d..c2f5b6e 100644 --- a/python_app/models/run_config_codec.py +++ b/python_app/models/run_config_codec.py @@ -179,7 +179,6 @@ def run_config_from_dict(payload: dict[str, Any]) -> RunConfigModel: if isinstance(band, (list, tuple)) and len(band) == 2: model.preprocess.notch.bands_hz.append((float(band[0]), float(band[1]))) - model.gpr.mode = str(gpr_payload.get("mode", model.gpr.mode)) model.gpr.relative_permittivity = float( gpr_payload.get("relative_permittivity", model.gpr.relative_permittivity) ) @@ -339,7 +338,6 @@ def run_config_to_dict(model: RunConfigModel) -> dict[str, Any]: }, }, "gpr": { - "mode": model.gpr.mode, "relative_permittivity": model.gpr.relative_permittivity, "tx_geometry": [ { diff --git a/python_app/models/run_config_schema.py b/python_app/models/run_config_schema.py index 07ab8ae..1623da2 100644 --- a/python_app/models/run_config_schema.py +++ b/python_app/models/run_config_schema.py @@ -183,7 +183,6 @@ class GprRxGeometryModel: class GprModel: """Stable GPR configuration saved in run_config.json.""" - mode: str = "point" relative_permittivity: float = 1.0 tx_geometry: list[GprTxGeometryModel] = field(default_factory=list) rx_geometry: list[GprRxGeometryModel] = field(default_factory=list) diff --git a/python_app/models/run_config_validation.py b/python_app/models/run_config_validation.py index ae2b343..6885f75 100644 --- a/python_app/models/run_config_validation.py +++ b/python_app/models/run_config_validation.py @@ -37,8 +37,6 @@ def validate_gpr_model( output_switch_positions: int, ) -> None: """Validate stable GPR config against current switch dimensions.""" - if gpr.mode not in {"point", "extended"}: - raise ValueError("gpr.mode must be either 'point' or 'extended'") if float(gpr.relative_permittivity) <= 0.0: raise ValueError("gpr.relative_permittivity must be > 0") diff --git a/python_app/orchestration/gpr_locator.py b/python_app/orchestration/gpr_locator.py index 97f2acb..feffcb1 100644 --- a/python_app/orchestration/gpr_locator.py +++ b/python_app/orchestration/gpr_locator.py @@ -50,7 +50,7 @@ def collection_has_gpr_payloads(collection: ResultCollection) -> bool: def gpr_object_rows(collection: ResultCollection) -> np.ndarray: - """Return object rows as `[x_m, z_m, pair_count]` from a GPR collection.""" + """Return object rows as `[x_m, z_m, score]` from a GPR collection.""" points_payload = collection_payload_by_name(collection, "gpr_points", kind=4) if points_payload is not None: points = np.asarray(points_payload.table, dtype=np.float32) @@ -68,17 +68,17 @@ def gpr_object_rows(collection: ResultCollection) -> np.ndarray: def locator_observations_from_collection( collection: ResultCollection, - min_pair_count: float, + min_score: float, *, visible_bounds: tuple[float, float, float, float] | None = None, ) -> list[dict[str, float]]: - """Build locator observations from GPR rows using pair threshold and optional X/Z bounds.""" + """Build locator observations from GPR rows using score threshold and optional X/Z bounds.""" rows = gpr_object_rows(collection) if rows.size == 0: return [] finite_mask = np.all(np.isfinite(rows[:, :3]), axis=1) - visible_mask = finite_mask & (rows[:, 2] >= float(min_pair_count)) + visible_mask = finite_mask & (rows[:, 2] >= float(min_score)) if visible_bounds is not None: x_min, x_max, z_min, z_max = (float(value) for value in visible_bounds) visible_mask &= ( @@ -90,7 +90,7 @@ def locator_observations_from_collection( filtered = rows[visible_mask] observations: list[dict[str, float]] = [] - for x_m, z_m, _pair_count in filtered: + for x_m, z_m, _score in filtered: observations.append( { "dst": round(float(z_m), 2), diff --git a/python_app/orchestration/live_processing_config.py b/python_app/orchestration/live_processing_config.py index 4b01126..3dd8e79 100644 --- a/python_app/orchestration/live_processing_config.py +++ b/python_app/orchestration/live_processing_config.py @@ -27,15 +27,13 @@ class ProcessingLiveConfig: gpr_output_positions: list[int] | None = None gpr_min_depth_m: float = 2.0 gpr_max_depth_m: float = 14.0 - gpr_comp_power: float = 0.2 + gpr_range_comp_power: float = 0.28 + gpr_angle_comp_power: float = 0.10 gpr_start_freq_mhz: float = 3000.0 gpr_stop_freq_mhz: float = 6000.0 - gpr_speed_m_s: float = 0.0 - gpr_look_angle_deg: float = 0.0 - gpr_snr_thresh: float = 4.5 - gpr_snr_comp_max: float = 25.0 gpr_background_subtract_enabled: bool = True gpr_background_mean_count: int = 10 + gpr_remove_sidelobe_objects_enabled: bool = True history_command_seq: int = 0 history_command: str = "none" @@ -69,15 +67,13 @@ class ProcessingLiveConfig: "gpr_output_positions": [int(value) for value in self.gpr_output_positions], "gpr_min_depth_m": float(self.gpr_min_depth_m), "gpr_max_depth_m": float(self.gpr_max_depth_m), - "gpr_comp_power": float(self.gpr_comp_power), + "gpr_range_comp_power": float(self.gpr_range_comp_power), + "gpr_angle_comp_power": float(self.gpr_angle_comp_power), "gpr_start_freq_mhz": float(self.gpr_start_freq_mhz), "gpr_stop_freq_mhz": float(self.gpr_stop_freq_mhz), - "gpr_speed_m_s": float(self.gpr_speed_m_s), - "gpr_look_angle_deg": float(self.gpr_look_angle_deg), - "gpr_snr_thresh": float(self.gpr_snr_thresh), - "gpr_snr_comp_max": float(self.gpr_snr_comp_max), "gpr_background_subtract_enabled": bool(self.gpr_background_subtract_enabled), "gpr_background_mean_count": int(self.gpr_background_mean_count), + "gpr_remove_sidelobe_objects_enabled": bool(self.gpr_remove_sidelobe_objects_enabled), "history_command_seq": int(self.history_command_seq), "history_command": str(self.history_command), } diff --git a/python_app/orchestration/locator_runtime.py b/python_app/orchestration/locator_runtime.py index e916e8b..664b33a 100644 --- a/python_app/orchestration/locator_runtime.py +++ b/python_app/orchestration/locator_runtime.py @@ -183,14 +183,14 @@ class LocatorTcpService: def publish_collection( self, collection: ResultCollection, - min_pair_count: float, + min_score: float, *, visible_bounds: tuple[float, float, float, float] | None = None, ) -> None: """Publish one locator payload derived from a GPR result collection.""" observations = locator_observations_from_collection( collection, - min_pair_count, + min_score, visible_bounds=visible_bounds, ) payload = build_locator_payload( diff --git a/run_config.json b/run_config.json index f168c8b..096fb14 100644 --- a/run_config.json +++ b/run_config.json @@ -1,20 +1,23 @@ { "radar": { - "model": "compact_m_k209", - "serial": "", + "model": "librevna_multi", + "serial": "207730885532", "remote_host": "127.0.0.1", "remote_port": 50209, "driver_mode": "native", "mock_signal_hz": 5000000.0, "multi_device": { - "slave_serials": [], - "force_external_reference": false, + "slave_serials": [ + "20A1307D5532", + "2072306C5532" + ], + "force_external_reference": true, "recovery_attempts": 3 }, "sweep": { "start_hz": 1000000.0, "stop_hz": 6000000000.0, - "points": 201, + "points": 4501, "if_bandwidth_hz": 50000.0, "stimulus_power_dbm": -10.0 } @@ -48,8 +51,8 @@ "run": { "settling_ms": 0, "idle_sleep_ms": 2, - "continuous": false, - "processing_live_config_path": "python_app/runtime/processing_live.json", + "continuous": true, + "processing_live_config_path": "/home/europa/Documents/radar_system/python_app/runtime/processing_live.json", "locator_server": { "device_id": 3, "protocol_version": 1, @@ -97,11 +100,11 @@ "preprocess": { "s21": { "calibration": { - "set_name": "", + "set_name": "set_001", "bundle_path": "" }, "reference": { - "set_name": "", + "set_name": "set_001", "bundle_path": "" } }, @@ -133,7 +136,6 @@ } }, "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ { @@ -166,29 +168,86 @@ }, "rings": { "raw": { - "name": "/radar_k209_local_raw", + "name": "/radar_raw", "capacity": 50, "slot_size_bytes": 2097152 }, "raw_tap": { - "name": "/radar_k209_local_raw_tap", + "name": "/radar_raw_tap", "capacity": 50, "slot_size_bytes": 2097152 }, "preprocessed": { - "name": "/radar_k209_local_preprocessed", + "name": "/radar_preprocessed", "capacity": 50, "slot_size_bytes": 2097152 }, "preprocessed_tap": { - "name": "/radar_k209_local_preprocessed_tap", + "name": "/radar_preprocessed_tap", "capacity": 50, "slot_size_bytes": 2097152 }, "results": { - "name": "/radar_k209_local_results", + "name": "/radar_results", "capacity": 50, "slot_size_bytes": 2097152 } + }, + "gui": { + "version": 1, + "switches": { + "combo_mode": "text", + "combos_text": "0:0,1:0,2:0,3:0,0:1,1:1,2:1,3:1", + "single_input": "0", + "single_output": "0" + }, + "processing": { + "selected_mode": "gpr", + "pass_through": { + "show_magnitude": true, + "show_phase": false, + "fixed_y_enabled": false, + "y_min_db": -100.0, + "y_max_db": 0.0 + }, + "bscan": { + "axis": "abs", + "cut_m": 0.0, + "max_depth_m": 3.0, + "gain": 1.0, + "start_freq_mhz": 100.0, + "stop_freq_mhz": 6000.0, + "subtract_mean_ascan": false + }, + "gpr": { + "input_positions": "0,1,2,3", + "output_positions": "0,1", + "min_depth_m": 2.0, + "max_depth_m": 14.0, + "range_comp_power": 0.28, + "angle_comp_power": 0.1, + "start_freq_mhz": 3000.0, + "stop_freq_mhz": 6000.0, + "background_subtract_enabled": true, + "background_mean_count": 10, + "remove_sidelobe_objects_enabled": false, + "render_mode": "heatmap", + "min_visible_score": 0.049999999999999684, + "visible_x_min_m": -1.609999999999999, + "visible_x_max_m": 1.1099999999999985, + "visible_z_min_m": 0.30000000000000004, + "visible_z_max_m": 14.0 + } + }, + "data_actions": { + "save_count": 10, + "save_path": "/home/europa/Documents/radar_system/python_app/data/snapshots", + "save_name": "snapshot_manual" + }, + "preprocess_dialog": { + "set_name": "set_001", + "radar_config_dir": "", + "use_all_radar_configs": false + } } -} +} \ No newline at end of file diff --git a/run_config_compact_m_k209.example.json b/run_config_compact_m_k209.example.json index 121601c..58dfb95 100644 --- a/run_config_compact_m_k209.example.json +++ b/run_config_compact_m_k209.example.json @@ -133,7 +133,6 @@ } }, "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ { diff --git a/run_config_compact_m_k209_local_mock_switches.example.json b/run_config_compact_m_k209_local_mock_switches.example.json index f168c8b..6238351 100644 --- a/run_config_compact_m_k209_local_mock_switches.example.json +++ b/run_config_compact_m_k209_local_mock_switches.example.json @@ -2,7 +2,7 @@ "radar": { "model": "compact_m_k209", "serial": "", - "remote_host": "127.0.0.1", + "remote_host": "192.168.8.102", "remote_port": 50209, "driver_mode": "native", "mock_signal_hz": 5000000.0, @@ -133,7 +133,6 @@ } }, "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ { diff --git a/run_config_librevna.example.json b/run_config_librevna.example.json index 33e0709..b887627 100644 --- a/run_config_librevna.example.json +++ b/run_config_librevna.example.json @@ -131,7 +131,6 @@ } }, "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ { diff --git a/run_config_librevna_multi.example.json b/run_config_librevna_multi.example.json index 8b57014..c7da2d1 100644 --- a/run_config_librevna_multi.example.json +++ b/run_config_librevna_multi.example.json @@ -134,7 +134,6 @@ } }, "gpr": { - "mode": "point", "relative_permittivity": 1.0, "tx_geometry": [ { diff --git a/start.sh b/start.sh index da3b1e4..0f1e13f 100755 --- a/start.sh +++ b/start.sh @@ -142,9 +142,9 @@ ensure_usb_access_rules() { cat > "${tmp_rule}" <<'EOF' # LibreVNA USB access for non-root users -SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="564e", TAG+="uaccess" -SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="4121", TAG+="uaccess" -SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="4121", TAG+="uaccess" +SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="564e", GROUP="plugdev", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="4121", GROUP="plugdev", MODE="0660", TAG+="uaccess" +SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="4121", GROUP="plugdev", MODE="0660", TAG+="uaccess" EOF if [[ -f "${rule_file}" ]] && cmp -s "${tmp_rule}" "${rule_file}"; then