awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

some fixes and improvements

Browse Source
This commit is contained in:
Ayzen
2026-05-28 14:33:12 +03:00
parent 83a934f251
commit eacea436a4
29 changed files with 2114 additions and 424 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp
+17
View File
@@ -18,6 +18,14 @@ enum class LegacyGprMode {
Extended,
};
enum class LegacyGprReferenceMode {
// t_ref = midpoint between the first and last event centers.
FrameCenter,
// t_ref = center of the first event. Useful when motion offsets should be
// accumulated from frame start, e.g. for tagging frames by their head time.
FirstTxEvent,
};
struct ProcessingLiveConfig {
std::string processor_mode = "pass_through";
std::string pass_through_channel = "s21";
@@ -42,6 +50,15 @@ struct ProcessingLiveConfig {
std::string gpr_score_mode = "combined";
float gpr_speed_m_s = 0.0F;
float gpr_look_angle_deg = 0.0F;
// Motion model parameters for the legacy GPR pipeline. The direction sign
// selects which way later Tx-events appear deeper (+1) or shallower (-1).
// Intra-sweep phase correction compensates the motion that happens *inside*
// one Tx-sweep before the IFFT — it is independent of the per-pair coarse
// tau shift and can be disabled without affecting the rest of the pipeline.
// The reference mode picks the anchor used to compute dt_ref per event.
float gpr_direction_sign = 1.0F;
bool gpr_apply_freq_phase_correction = true;
LegacyGprReferenceMode gpr_reference_mode = LegacyGprReferenceMode::FrameCenter;
float gpr_snr_thresh = 4.5F;
float gpr_snr_comp_max = 25.0F;
float gpr_start_freq_mhz = 3000.0F;
data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp
+31
View File
@@ -41,6 +41,18 @@ using Json = nlohmann::json;
throw std::runtime_error(field_name + " must be one of: point, extended");
}
[[nodiscard]] auto parse_legacy_gpr_reference_mode(
const std::string& value, const std::string& field_name
) -> LegacyGprReferenceMode {
if (value == "frame_center") {
return LegacyGprReferenceMode::FrameCenter;
}
if (value == "first_tx_event") {
return LegacyGprReferenceMode::FirstTxEvent;
}
throw std::runtime_error(field_name + " must be one of: frame_center, first_tx_event");
}
[[nodiscard]] auto parse_gpr_score_mode(const std::string& value, const std::string& field_name) -> std::string {
if (value == "peak" || value == "combined") {
return value;
@@ -266,6 +278,25 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
}
config.gpr_look_angle_deg = static_cast<float>(found->get<double>());
}
if (const auto found = root.find("gpr_direction_sign"); found != root.end()) {
if (!found->is_number()) {
throw std::runtime_error("processing.gpr_direction_sign must be number");
}
config.gpr_direction_sign = static_cast<float>(found->get<double>());
}
if (const auto found = root.find("gpr_apply_freq_phase_correction"); found != root.end()) {
if (!found->is_boolean()) {
throw std::runtime_error("processing.gpr_apply_freq_phase_correction must be bool");
}
config.gpr_apply_freq_phase_correction = found->get<bool>();
}
if (const auto found = root.find("gpr_reference_mode"); found != root.end()) {
if (!found->is_string()) {
throw std::runtime_error("processing.gpr_reference_mode must be string");
}
config.gpr_reference_mode =
parse_legacy_gpr_reference_mode(found->get<std::string>(), "processing.gpr_reference_mode");
}
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");
data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp
+310 -74
View File
@@ -1,3 +1,37 @@
// Legacy MIMO GPR — ellipse-intersection localizer with motion compensation.
//
// This translation unit is included from `gpr_processor.cpp` *after*
// `gpr_backprojection_processor.ipp`, which defines the shared building blocks
// (kPi, SelectedTrace, GeometrySelection, distance_3d, fft_inplace, ...). Do
// not include this file directly.
//
// Pipeline overview (mirrors the Python reference Ellips_motion_remake_2.py):
// 1. Pre-process each pair: optional background subtraction (already done in
// collect_selected_traces); optional intra-sweep phase correction of S21
// before the IFFT — compensates radar displacement that happens *inside*
// one sweep where the frequencies are stepped linearly in time.
// 2. IFFT each pair → A-scan; find peaks above an SNR threshold within the
// depth gate; record both raw and attenuation-compensated SNR.
// 3. Apply coarse per-event motion correction so every peak carries a
// motion-corrected (tau_corr, z_corr) on top of the apparent values.
// 4. CLEAN-style iterative ellipse intersection: build a soft Gaussian-shell
// accumulator, take the strongest pixel, count agreeing pairs, suppress
// its depth band, repeat.
// 5. Optional extended-mode region detection for diffuse reflectors.
//
// Sweep-event model — radar topology matters:
// * Matrix radars (`librevna_multi`, `sn9000`) fire one Tx at a time and
// receive on all Rx channels in parallel. A run of 8 traces is just 2
// Tx-events; all (tx_k, *) pairs share one timestamp.
// * Sequential radars (single librevna with switches, kamil_adc, k209…)
// measure each pair separately. A run of 8 traces is 8 separate sweeps;
// every pair has its own timestamp.
// Both cases reduce to: there are N events in the frame, each event lasts
// `event_duration_s = (capture_end_ns - capture_start_ns) / N`. What changes
// is how event indices are assigned to pairs — by Tx for matrix radars, by
// trace run-order for sequential ones. `event_duration_s` is derived from
// the collection metadata, never from a live-config knob.
constexpr double kLegacyGridZMinM = 0.20;
constexpr double kLegacySmoothSigma = 3.0;
constexpr double kLegacyCleanSuppressRadiusM = 0.07;
@@ -5,6 +39,7 @@ constexpr double kLegacyCleanThresholdFrac = 0.05;
constexpr std::size_t kLegacyMaxObjects = 15U;
constexpr double kLegacyExtendedThresholdFrac = 0.75;
constexpr double kLegacyExtendedMinAreaCm2 = 2.0;
constexpr double kLegacyAttenuationReferenceDepthM = 3.0;
struct LegacyAscanResult {
std::vector<double> time_s{};
@@ -36,8 +71,26 @@ struct LegacyRegionRecord {
std::vector<float> mask{};
};
struct LegacyPairTiming {
double dtau_motion_s = 0.0;
// Per-pair timing snapshot. For matrix radars, all (tx_k, *) pairs share the
// same row; for sequential radars every pair has a distinct row. Indexing by
// pair keeps the rest of the pipeline ignorant of the radar topology.
struct LegacyEventTiming {
std::size_t event_index = 0U; // 0-based order of the event in the frame
double t_start_s = 0.0; // start of this event sweep relative to frame
double t_center_s = 0.0; // center of this event sweep
double dt_ref_s = 0.0; // t_center_s - t_frame_ref_s
double dz_motion_m = 0.0; // direction_sign * speed * dt_ref * cos(theta)
double dtau_motion_s = 0.0; // 2 * dz_motion_m / velocity
};
struct LegacyMotionTiming {
double event_duration_s = 0.0; // (capture_end - capture_start) / num_events
double cos_look_angle = 1.0;
double direction_sign = 1.0;
double speed_m_s = 0.0;
bool apply_intra_sweep_phase = false;
bool parallel_rx_per_tx_event = false;
std::unordered_map<PairKey, LegacyEventTiming> by_pair{};
};
enum class LegacyPeakDomain {
@@ -90,20 +143,31 @@ enum class LegacyPeakDomain {
return selected;
}
// Expected geo*pattern attenuation for a target directly under the virtual
// pair center at depth `z_app`. Uses full 3D antenna coordinates so this
// generalizes to non-coplanar antenna layouts; boresight is taken along +Z.
[[nodiscard]] auto legacy_attenuation_at_depth(
std::size_t tx_index,
std::size_t rx_index,
double z_app,
const std::vector<double>& x_tx,
const std::vector<double>& x_rx
const GeometrySelection& selection,
double imaging_plane_y_m
) -> double {
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], z_app);
const double r_rx = std::hypot(x_center - x_rx[rx_index], z_app);
const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
const double r_tx = distance_3d(
x_center - selection.x_tx[tx_index],
imaging_plane_y_m - selection.y_tx[tx_index],
z_app - selection.z_tx[tx_index]
);
const double r_rx = distance_3d(
x_center - selection.x_rx[rx_index],
imaging_plane_y_m - selection.y_rx[rx_index],
z_app - selection.z_rx[rx_index]
);
const double cos_tx = (z_app - selection.z_tx[tx_index]) / (r_tx + 1e-12);
const double cos_rx = (z_app - selection.z_rx[rx_index]) / (r_rx + 1e-12);
const double geo = 1.0 / ((r_tx * r_rx) + 1e-12);
const double pattern =
std::pow(z_app / (r_tx + 1e-12), 2.0) *
std::pow(z_app / (r_rx + 1e-12), 2.0);
const double pattern = (cos_tx * cos_tx) * (cos_rx * cos_rx);
return (geo * pattern) + 1e-30;
}
@@ -203,67 +267,193 @@ enum class LegacyPeakDomain {
return false;
}
[[nodiscard]] auto build_legacy_motion_timing_by_pair(
const std::vector<SelectedTrace>& traces,
std::size_t total_combo_count,
// True when `z_value` should participate in the ellipse vote — both the static
// depth gate ([min, max]) and the per-step CLEAN-suppression bands must allow it.
// Mirrors Python's `_peak_in_work_depth` + the per-step `excl_z` filter.
[[nodiscard]] auto is_legacy_depth_active(
double z_value,
double min_depth_m,
double max_depth_m,
const std::vector<std::pair<double, double>>& excluded_ranges
) -> bool {
if (z_value < min_depth_m || z_value > max_depth_m) {
return false;
}
return !is_legacy_depth_excluded(z_value, excluded_ranges);
}
[[nodiscard]] auto is_matrix_radar_model(const std::string& model) -> bool {
return model == "librevna_multi" || model == "sn9000";
}
// Assign an `event_index` to every selected pair. The mapping depends on the
// radar topology:
// * Matrix radar — all (tx_k, *) pairs share one event, ordered by the Tx's
// first appearance in run order. So 8 traces with 2 Tx's give 2 events.
// * Sequential radar — every pair is its own event, ordered by run order.
// So 8 traces give 8 events.
[[nodiscard]] auto assign_event_indices(
const std::vector<SelectedTrace>& selected_traces,
bool matrix_radar
) -> std::pair<std::unordered_map<PairKey, std::size_t>, std::size_t> {
std::unordered_map<PairKey, std::size_t> event_index_by_pair{};
event_index_by_pair.reserve(selected_traces.size());
if (matrix_radar) {
std::unordered_map<std::uint32_t, std::size_t> event_by_tx{};
std::size_t next_event = 0U;
for (const auto& trace : selected_traces) {
const auto [event_it, inserted] = event_by_tx.try_emplace(trace.tx_local_index, next_event);
if (inserted) {
++next_event;
}
event_index_by_pair[make_pair_key(trace.tx_local_index, trace.rx_local_index)] = event_it->second;
}
return {std::move(event_index_by_pair), next_event};
}
// Sequential mode: rank traces by their run_order so the event index is a
// dense 0..N-1 sequence regardless of any holes in run_order.
std::vector<std::pair<std::size_t, PairKey>> ordered{};
ordered.reserve(selected_traces.size());
for (const auto& trace : selected_traces) {
ordered.emplace_back(trace.run_order, make_pair_key(trace.tx_local_index, trace.rx_local_index));
}
std::sort(ordered.begin(), ordered.end(),
[](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
for (std::size_t event_index = 0U; event_index < ordered.size(); ++event_index) {
event_index_by_pair[ordered[event_index].second] = event_index;
}
return {std::move(event_index_by_pair), ordered.size()};
}
// Build per-pair motion timing. `event_duration_s` is derived from collection
// metadata as `(capture_end_ns - capture_start_ns) / num_events` — it is the
// duration of one sweep event in the frame, never a live-config knob. If the
// motion model is disabled (speed = 0 and phase correction off), the function
// still returns one row per pair so downstream code can index uniformly.
[[nodiscard]] auto compute_legacy_motion_timing(
const std::vector<SelectedTrace>& selected_traces,
bool matrix_radar,
std::uint64_t capture_start_ns,
std::uint64_t capture_end_ns,
const ProcessingLiveConfig& live_config,
double velocity_mps
) -> std::unordered_map<PairKey, LegacyPairTiming> {
std::unordered_map<PairKey, LegacyPairTiming> timing_by_pair{};
timing_by_pair.reserve(traces.size());
) -> LegacyMotionTiming {
LegacyMotionTiming timing{};
timing.speed_m_s = static_cast<double>(live_config.gpr_speed_m_s);
timing.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
timing.cos_look_angle = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
timing.apply_intra_sweep_phase = live_config.gpr_apply_freq_phase_correction;
timing.parallel_rx_per_tx_event = matrix_radar;
const double speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
if (!(std::abs(speed_mps) > 1e-12)) {
for (const auto& trace : traces) {
timing_by_pair.emplace(make_pair_key(trace.tx_local_index, trace.rx_local_index), LegacyPairTiming{});
if (selected_traces.empty()) {
return timing;
}
auto [event_index_by_pair, num_events] = assign_event_indices(selected_traces, matrix_radar);
if (num_events == 0U) {
return timing;
}
const bool speed_meaningful = std::abs(timing.speed_m_s) > 1e-12;
const bool model_active = speed_meaningful || timing.apply_intra_sweep_phase;
// No motion and no phase correction — populate with zeroed rows and bail.
if (!model_active) {
for (const auto& [pair_key, event_index] : event_index_by_pair) {
timing.by_pair.emplace(pair_key, LegacyEventTiming{.event_index = event_index});
}
return timing_by_pair;
return timing;
}
if (total_combo_count == 0U) {
throw std::runtime_error("Legacy GPR requires at least one run combo");
}
if (capture_end_ns <= capture_start_ns) {
throw std::runtime_error(
"Legacy GPR requires valid capture_start_ns/capture_end_ns metadata when speed is non-zero"
"Legacy GPR motion model requires valid capture_start_ns/capture_end_ns metadata"
);
}
const double capture_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
const double slot_duration_s = capture_span_s / static_cast<double>(total_combo_count);
if (!(slot_duration_s > 0.0)) {
throw std::runtime_error("Legacy GPR requires positive collection capture span when speed is non-zero");
const double total_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
const double event_duration_s = total_span_s / static_cast<double>(num_events);
if (!(event_duration_s > 0.0)) {
throw std::runtime_error("Legacy GPR motion model requires positive per-event duration");
}
timing.event_duration_s = event_duration_s;
// Reference anchor for dt_ref: either the midpoint between the first and
// last event centers (frame_center) or just the first event center
// (first_tx_event). Matches Python's `MOTION_CONFIG.reference_mode`.
const double first_center_s = 0.5 * event_duration_s;
const double last_center_s = (static_cast<double>(num_events) - 0.5) * event_duration_s;
const double t_ref_s = live_config.gpr_reference_mode == LegacyGprReferenceMode::FirstTxEvent
? first_center_s
: 0.5 * (first_center_s + last_center_s);
const double motion_factor = speed_meaningful
? timing.direction_sign * timing.speed_m_s * timing.cos_look_angle
: 0.0;
for (const auto& [pair_key, event_index] : event_index_by_pair) {
LegacyEventTiming row{};
row.event_index = event_index;
row.t_start_s = static_cast<double>(event_index) * event_duration_s;
row.t_center_s = row.t_start_s + (0.5 * event_duration_s);
row.dt_ref_s = row.t_center_s - t_ref_s;
row.dz_motion_m = motion_factor * row.dt_ref_s;
row.dtau_motion_s = (2.0 * row.dz_motion_m) / velocity_mps;
timing.by_pair.emplace(pair_key, row);
}
const double t_ref_s = 0.5 * capture_span_s;
const double cos_theta = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
for (const auto& trace : traces) {
const double t_center_s = (static_cast<double>(trace.run_order) + 0.5) * slot_duration_s;
const double dz_motion_m = speed_mps * (t_center_s - t_ref_s) * cos_theta;
timing_by_pair.emplace(
make_pair_key(trace.tx_local_index, trace.rx_local_index),
LegacyPairTiming{.dtau_motion_s = (2.0 * dz_motion_m) / velocity_mps}
);
return timing;
}
// Compensate for the radar moving while a single sweep is being recorded.
// Frequencies inside one sweep are stepped linearly in time, so each frequency
// is sampled from a slightly different antenna position. The correction shifts
// each frequency's phase back to the event center; after that the IFFT
// produces an A-scan as if the whole sweep were captured at one position.
void apply_intra_sweep_phase_correction(
SelectedTrace& trace,
const LegacyEventTiming& timing,
const LegacyMotionTiming& motion,
double velocity_mps
) {
if (!motion.apply_intra_sweep_phase || !(motion.event_duration_s > 0.0)) {
return;
}
if (!(std::abs(motion.speed_m_s) > 1e-12)) {
return; // No motion → zero phase shift, no-op.
}
const std::size_t point_count = trace.frequency_hz.size();
if (point_count < 2U || trace.s21.size() != point_count) {
return;
}
return timing_by_pair;
const double dt_freq_s = motion.event_duration_s / static_cast<double>(point_count - 1U);
const double motion_factor = motion.direction_sign * motion.speed_m_s * motion.cos_look_angle;
for (std::size_t index = 0U; index < point_count; ++index) {
const double t_abs_s = timing.t_start_s + (static_cast<double>(index) * dt_freq_s);
const double dt_intra_s = t_abs_s - timing.t_center_s;
const double delta_path_m = 2.0 * motion_factor * dt_intra_s;
const double phi = (2.0 * kPi * trace.frequency_hz[index] * delta_path_m) / velocity_mps;
trace.s21[index] *= std::polar(1.0, phi);
}
}
void apply_legacy_motion_correction(
std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
const std::unordered_map<PairKey, LegacyPairTiming>& timing_by_pair,
const LegacyMotionTiming& motion,
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 legacy GPR combo");
const auto timing_it = motion.by_pair.find(key);
if (timing_it == motion.by_pair.end()) {
throw std::runtime_error("Missing motion timing for selected legacy GPR pair");
}
const double dtau_motion_s = timing_it->second.dtau_motion_s;
for (auto& peak : peaks) {
peak.tau_corr = peak.tau + timing_it->second.dtau_motion_s;
peak.tau_corr = peak.tau + dtau_motion_s;
peak.z_corr = 0.5 * velocity_mps * peak.tau_corr;
}
}
@@ -275,6 +465,8 @@ void apply_legacy_motion_correction(
const std::vector<std::pair<double, double>>& exclude_ranges,
double velocity_mps,
double shell_sigma_m,
double min_depth_m,
double max_depth_m,
const std::vector<double>& x_tx,
const std::vector<double>& x_rx,
LegacyPeakDomain domain
@@ -298,7 +490,8 @@ void apply_legacy_motion_correction(
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_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
const double depth = legacy_peak_depth_for_domain(peak, domain);
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
continue;
}
@@ -324,6 +517,8 @@ void apply_legacy_motion_correction(
const std::vector<double>& x_rx,
double velocity_mps,
double shell_sigma_m,
double min_depth_m,
double max_depth_m,
LegacyPeakDomain domain
) -> double {
std::size_t count = 0U;
@@ -337,7 +532,8 @@ void apply_legacy_motion_correction(
}
for (const auto& peak : peak_it->second) {
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
const double depth = legacy_peak_depth_for_domain(peak, domain);
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
continue;
}
@@ -405,22 +601,26 @@ void apply_legacy_motion_correction(
const std::vector<double>& x_rx,
double velocity_mps,
double shell_sigma_m,
double min_depth_m,
double max_depth_m,
LegacyPeakDomain domain
) -> std::pair<std::vector<LegacyPointRecord>, std::vector<double>> {
std::vector<LegacyPointRecord> found{};
const auto accumulator =
build_legacy_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx, domain);
const auto accumulator = build_legacy_accumulator(
grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, min_depth_m, max_depth_m, x_tx, x_rx, domain
);
const double initial_max = max_value(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(), kLegacySmoothSigma)};
}
// Suppression radius rounds down to mirror Python's `int(0.07 / dx)`.
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::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
const auto radius_z =
static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
std::vector<std::pair<double, double>> excluded_ranges{};
for (std::size_t step = 0U; step < kLegacyMaxObjects; ++step) {
@@ -430,6 +630,8 @@ void apply_legacy_motion_correction(
excluded_ranges,
velocity_mps,
shell_sigma_m,
min_depth_m,
max_depth_m,
x_tx,
x_rx,
domain
@@ -469,11 +671,16 @@ void apply_legacy_motion_correction(
x_rx,
velocity_mps,
shell_sigma_m,
min_depth_m,
max_depth_m,
domain
),
}
);
// Collect depths of all peaks consistent with the just-detected point;
// they form the next exclusion band so subsequent CLEAN steps cannot
// re-pick the same target.
std::vector<double> 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) {
@@ -484,14 +691,15 @@ void apply_legacy_motion_correction(
continue;
}
for (const auto& peak : peak_it->second) {
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), excluded_ranges)) {
const double depth = legacy_peak_depth_for_domain(peak, domain);
if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, excluded_ranges)) {
continue;
}
const double r_tx = std::hypot(x_est - x_tx[tx_index], z_est);
const double r_rx = std::hypot(x_est - x_rx[rx_index], z_est);
if (std::abs((r_tx + r_rx) - (velocity_mps * legacy_peak_tau_for_domain(peak, domain))) <
shell_sigma_m * 3.0) {
matched_depths.push_back(legacy_peak_depth_for_domain(peak, domain));
matched_depths.push_back(depth);
}
}
}
@@ -512,7 +720,9 @@ void apply_legacy_motion_correction(
const std::vector<double>& x_tx,
const std::vector<double>& x_rx,
double velocity_mps,
double shell_sigma_m
double shell_sigma_m,
double min_depth_m,
double max_depth_m
) -> std::pair<std::vector<LegacyRegionRecord>, std::vector<double>> {
std::vector<LegacyRegionRecord> regions{};
const auto accumulator = build_legacy_accumulator(
@@ -521,6 +731,8 @@ void apply_legacy_motion_correction(
{},
velocity_mps,
shell_sigma_m,
min_depth_m,
max_depth_m,
x_tx,
x_rx,
LegacyPeakDomain::Apparent
@@ -622,6 +834,8 @@ void apply_legacy_motion_correction(
x_rx,
velocity_mps,
shell_sigma_m,
min_depth_m,
max_depth_m,
LegacyPeakDomain::Apparent
);
region.pixel_count = static_cast<double>(component.size());
@@ -649,7 +863,7 @@ void apply_legacy_motion_correction(
validate_collection_trace_order(run_config, collection);
const auto background_mean = build_background_mean(previous_collections, selection, live_config);
const auto selected_traces = collect_selected_traces(collection, selection, background_mean);
auto selected_traces = collect_selected_traces(collection, selection, background_mean);
if (selected_traces.empty()) {
return results;
}
@@ -663,6 +877,32 @@ void apply_legacy_motion_correction(
if (!(max_depth_m > min_depth_m)) {
return results;
}
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
// Motion timing is computed once per collection. Matrix radars get one
// event per Tx (parallel Rx); sequential radars get one event per pair.
const bool matrix_radar = is_matrix_radar_model(run_config.radar.model);
const auto motion_timing = compute_legacy_motion_timing(
selected_traces,
matrix_radar,
collection.capture_start_ns,
collection.capture_end_ns,
live_config,
velocity_mps
);
// Intra-sweep phase correction (frequency-domain) — happens BEFORE the IFFT
// because it modifies the S21 spectrum that compute_legacy_ascan consumes.
if (motion_timing.apply_intra_sweep_phase) {
for (auto& trace : selected_traces) {
const auto pair_key = make_pair_key(trace.tx_local_index, trace.rx_local_index);
const auto timing_it = motion_timing.by_pair.find(pair_key);
if (timing_it == motion_timing.by_pair.end()) {
continue;
}
apply_intra_sweep_phase_correction(trace, timing_it->second, motion_timing, velocity_mps);
}
}
std::unordered_map<PairKey, LegacyAscanResult> ascans_by_pair{};
double bandwidth_hz = 0.0;
@@ -678,12 +918,7 @@ void apply_legacy_motion_correction(
return results;
}
const auto grid = build_grid(
selection,
max_depth_m,
kLegacyGridZMinM,
static_cast<double>(live_config.gpr_imaging_plane_y_m)
);
const auto grid = build_grid(selection, max_depth_m, kLegacyGridZMinM, imaging_plane_y_m);
if (grid.x_grid.empty() || grid.z_grid.empty()) {
return results;
}
@@ -724,15 +959,17 @@ void apply_legacy_motion_correction(
const auto peak_indices =
find_legacy_peak_indices(ascan.amplitude, min_index, max_index, noise * snr_thresh, min_distance);
const double attenuation_ref = legacy_attenuation_at_depth(
tx_index, rx_index, kLegacyAttenuationReferenceDepthM, selection, imaging_plane_y_m
);
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 =
legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection.x_tx, selection.x_rx);
const double attenuation_ref =
legacy_attenuation_at_depth(tx_index, rx_index, 3.0, selection.x_tx, selection.x_rx);
legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection, imaging_plane_y_m);
const double snr_comp = std::min(
snr_raw / (std::pow(attenuation / attenuation_ref, comp_power) + 1e-12),
snr_comp_max
@@ -762,7 +999,9 @@ void apply_legacy_motion_correction(
selection.x_tx,
selection.x_rx,
velocity_mps,
shell_sigma_m
shell_sigma_m,
min_depth_m,
max_depth_m
);
results.collection_payloads.push_back(
build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator)
@@ -793,15 +1032,10 @@ void apply_legacy_motion_correction(
return results;
}
const auto motion_timing_by_pair = build_legacy_motion_timing_by_pair(
selected_traces,
run_config.run_combos.size(),
collection.capture_start_ns,
collection.capture_end_ns,
live_config,
velocity_mps
);
apply_legacy_motion_correction(peaks_by_pair, motion_timing_by_pair, velocity_mps);
// Apply coarse per-Tx-event motion correction to the peak set produced
// above. After this step every peak carries both apparent and motion-
// corrected (tau, depth) values; the CLEAN search uses the corrected domain.
apply_legacy_motion_correction(peaks_by_pair, motion_timing, velocity_mps);
const auto [points, smoothed_accumulator] = clean_legacy_find_points(
grid,
@@ -810,6 +1044,8 @@ void apply_legacy_motion_correction(
selection.x_rx,
velocity_mps,
shell_sigma_m,
min_depth_m,
max_depth_m,
LegacyPeakDomain::Corrected
);
results.collection_payloads.push_back(