added gpr speed tracking

data_acq_and_processing/processing/processors/src/gpr_processor.cpp

@@ -9,6 +9,7 @@
 #include <numeric>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -18,9 +19,9 @@ namespace {
 constexpr double kPi = 3.14159265358979323846;
 constexpr double kSpeedOfLightMetersPerSec = 299'792'458.0;
 constexpr double kAccumulatorXMarginM = 2.0;
-constexpr double kAccumulatorZMinM = 0.10;
-constexpr double kSnrThresh = 3.0;
-constexpr double kSnrCompMax = 20.0;
+constexpr double kAccumulatorZMinM = 0.20;
+constexpr double kSnrThresh = 4.5;
+constexpr double kSnrCompMax = 25.0;
 constexpr std::size_t kGridWidth = 300U;
 constexpr std::size_t kGridHeight = 300U;
 constexpr double kGaussianSigma = 3.0;
@@ -33,6 +34,10 @@ constexpr double kExtendedMinAreaCm2 = 2.0;
 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<double> frequency_hz{};
     std::vector<std::complex<double>> s21{};
 };
@@ -47,6 +52,8 @@ struct AscanResult {
 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;
 };
@@ -79,6 +86,12 @@ 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 GridDefinition {
     std::vector<double> x_grid{};
     std::vector<double> z_grid{};
@@ -86,10 +99,19 @@ struct GridDefinition {
     std::vector<std::vector<double>> rx_distance_grids{};
 };
 
+enum class PeakDomain {
+    Apparent,
+    Corrected,
+};
+
 [[nodiscard]] auto make_pair_key(std::uint32_t tx_local_index, std::uint32_t rx_local_index) -> PairKey {
     return (static_cast<PairKey>(tx_local_index) << 32U) | static_cast<PairKey>(rx_local_index);
 }
 
+[[nodiscard]] auto combo_to_string(const ipc::ComboKey& combo) -> std::string {
+    return "input=" + std::to_string(combo.input_pos) + " output=" + std::to_string(combo.output_pos);
+}
+
 [[nodiscard]] auto next_power_of_two(std::size_t value) -> std::size_t {
     if (value <= 1U) {
         return 1U;
@@ -393,6 +415,32 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     return selection;
 }
 
+void validate_collection_trace_order(
+    const config::RunConfig& run_config,
+    const ipc::PreprocessedCollection& collection
+) {
+    if (collection.traces.size() != run_config.run_combos.size()) {
+        throw std::runtime_error(
+            "GPR requires collection trace order to match run.combos exactly: trace_count=" +
+            std::to_string(collection.traces.size()) +
+            ", run_combo_count=" + std::to_string(run_config.run_combos.size())
+        );
+    }
+
+    for (std::size_t index = 0U; index < collection.traces.size(); ++index) {
+        const auto& actual = collection.traces[index].combo;
+        const auto& expected = run_config.run_combos[index];
+        if (actual.input_pos == expected.input_pos && actual.output_pos == expected.output_pos) {
+            continue;
+        }
+
+        throw std::runtime_error(
+            "GPR requires preprocessed trace order to match run.combos: index=" + std::to_string(index) +
+            ", expected=(" + combo_to_string(expected) + "), actual=(" + combo_to_string(actual) + ")"
+        );
+    }
+}
+
 [[nodiscard]] auto build_background_mean(
     std::span<const ipc::PreprocessedCollection> previous_collections,
     const GeometrySelection& selection,
@@ -451,13 +499,17 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
 }
 
 [[nodiscard]] auto collect_selected_traces(
+    const config::RunConfig& run_config,
     const ipc::PreprocessedCollection& collection,
     const GeometrySelection& selection,
     const std::unordered_map<PairKey, std::vector<std::complex<double>>>& background_mean
-) -> std::unordered_map<PairKey, SelectedTrace> {
-    std::unordered_map<PairKey, SelectedTrace> traces{};
+) -> std::vector<SelectedTrace> {
+    std::vector<SelectedTrace> traces{};
+    traces.reserve(collection.traces.size());
+    std::unordered_set<PairKey> seen_keys{};
 
-    for (const auto& trace : collection.traces) {
+    for (std::size_t trace_index = 0U; trace_index < collection.traces.size(); ++trace_index) {
+        const auto& trace = collection.traces[trace_index];
         const auto output_it = selection.output_local_by_pos.find(trace.combo.output_pos);
         const auto input_it = selection.input_local_by_pos.find(trace.combo.input_pos);
         if (output_it == selection.output_local_by_pos.end() || input_it == selection.input_local_by_pos.end()) {
@@ -465,13 +517,23 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
         }
 
         SelectedTrace selected{};
+        selected.combo = trace.combo;
+        selected.tx_local_index = output_it->second;
+        selected.rx_local_index = input_it->second;
+        selected.run_order = trace_index;
         selected.frequency_hz.reserve(trace.frequency_hz.size());
         for (const auto value : trace.frequency_hz) {
             selected.frequency_hz.push_back(static_cast<double>(value));
         }
 
         selected.s21.reserve(trace.s21.size());
-        const auto key = make_pair_key(output_it->second, input_it->second);
+        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])
+            );
+        }
         const auto background_it = background_mean.find(key);
         for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) {
             std::complex<double> sample(trace.s21[sample_index].re, trace.s21[sample_index].im);
@@ -481,7 +543,7 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
             selected.s21.push_back(sample);
         }
 
-        traces[key] = std::move(selected);
+        traces.push_back(std::move(selected));
     }
 
     return traces;
@@ -618,6 +680,91 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     return false;
 }
 
+[[nodiscard]] auto peak_depth_for_domain(const PeakRecord& peak, PeakDomain domain) -> double {
+    return domain == PeakDomain::Corrected ? peak.z_corr : peak.z_app;
+}
+
+[[nodiscard]] auto peak_tau_for_domain(const PeakRecord& peak, PeakDomain domain) -> double {
+    return domain == PeakDomain::Corrected ? peak.tau_corr : peak.tau;
+}
+
+[[nodiscard]] auto build_motion_timing_by_pair(
+    const std::vector<SelectedTrace>& 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<PairKey, PairTiming> {
+    std::unordered_map<PairKey, PairTiming> timing_by_pair{};
+    timing_by_pair.reserve(traces.size());
+
+    const double speed_m_s = static_cast<double>(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<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(
+            "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<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 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;
+}
+
+void apply_motion_correction(
+    std::unordered_map<PairKey, std::vector<PeakRecord>>& peaks_by_pair,
+    const std::unordered_map<PairKey, PairTiming>& 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(
     const GridDefinition& grid,
     const std::unordered_map<PairKey, std::vector<PeakRecord>>& peaks_by_pair,
@@ -625,7 +772,8 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     double velocity_mps,
     double shell_sigma_m,
     const std::vector<double>& x_tx,
-    const std::vector<double>& x_rx
+    const std::vector<double>& x_rx,
+    PeakDomain domain
 ) -> std::vector<double> {
     const std::size_t width = grid.x_grid.size();
     const std::size_t height = grid.z_grid.size();
@@ -644,11 +792,11 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
             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.z_app, exclude_ranges)) {
+                if (is_excluded(peak_depth_for_domain(peak, domain), exclude_ranges)) {
                     continue;
                 }
 
-                const double range_total = velocity_mps * peak.tau;
+                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));
@@ -669,7 +817,8 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     const std::vector<double>& x_tx,
     const std::vector<double>& x_rx,
     double velocity_mps,
-    double shell_sigma_m
+    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) {
@@ -680,13 +829,13 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
             }
 
             for (const auto& peak : peak_it->second) {
-                if (is_excluded(peak.z_app, exclude_ranges)) {
+                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)) < shell_sigma_m * 6.0) {
+                if (std::abs((rt + rr) - (velocity_mps * peak_tau_for_domain(peak, domain))) < shell_sigma_m * 6.0) {
                     count += 1U;
                     break;
                 }
@@ -800,10 +949,11 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     const std::vector<double>& x_tx,
     const std::vector<double>& x_rx,
     double velocity_mps,
-    double shell_sigma_m
+    double shell_sigma_m,
+    PeakDomain domain
 ) -> std::pair<std::vector<PointRecord>, std::vector<double>> {
     std::vector<PointRecord> found{};
-    const auto accumulator = build_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx);
+    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)};
@@ -816,7 +966,16 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
 
     std::vector<std::pair<double, double>> 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);
+        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))) {
@@ -842,7 +1001,17 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
         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);
+        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<double> matched_depths{};
@@ -853,13 +1022,13 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
                     continue;
                 }
                 for (const auto& peak : peak_it->second) {
-                    if (is_excluded(peak.z_app, excluded_ranges)) {
+                    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)) < shell_sigma_m * 3.0) {
-                        matched_depths.push_back(peak.z_app);
+                    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));
                     }
                 }
             }
@@ -883,7 +1052,16 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     double shell_sigma_m
 ) -> std::pair<std::vector<RegionRecord>, std::vector<double>> {
     std::vector<RegionRecord> regions{};
-    const auto accumulator = build_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx);
+    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) {
@@ -968,7 +1146,17 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
 
             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);
+            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<double>(component.size());
             regions.push_back(std::move(region));
         }
@@ -998,9 +1186,10 @@ auto GprProcessor::process_collection(
         return results;
     }
 
+    validate_collection_trace_order(run_config, collection);
     const auto background_mean = build_background_mean(previous_collections, selection, live_config);
-    const auto traces_by_pair = collect_selected_traces(collection, selection, background_mean);
-    if (traces_by_pair.empty()) {
+    const auto selected_traces = collect_selected_traces(run_config, collection, selection, background_mean);
+    if (selected_traces.empty()) {
         return results;
     }
 
@@ -1011,7 +1200,8 @@ auto GprProcessor::process_collection(
 
     std::unordered_map<PairKey, AscanResult> ascans_by_pair{};
     double bandwidth_hz = 0.0;
-    for (const auto& [key, trace] : traces_by_pair) {
+    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)) {
             continue;
@@ -1044,13 +1234,15 @@ auto GprProcessor::process_collection(
                 continue;
             }
 
-            const double noise = median_copy(ascan.amplitude);
             const auto min_index = lower_bound_index(ascan.depth_m, static_cast<double>(live_config.gpr_min_depth_m));
             const auto max_index = lower_bound_index(ascan.depth_m, static_cast<double>(live_config.gpr_max_depth_m));
             if (max_index <= min_index + 2U) {
                 continue;
             }
 
+            const double noise =
+                median_copy(std::vector<double>(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::size_t>(std::max(
                 4.0,
@@ -1064,7 +1256,7 @@ auto GprProcessor::process_collection(
                 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, 2.0, selection.x_tx, selection.x_rx);
+                    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<double>(live_config.gpr_comp_power)) + 1e-12),
                     kSnrCompMax
@@ -1072,6 +1264,8 @@ auto GprProcessor::process_collection(
                 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,
                 });
@@ -1121,13 +1315,24 @@ auto GprProcessor::process_collection(
         return results;
     }
 
+    const auto motion_timing_by_pair = build_motion_timing_by_pair(
+        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(
         grid,
         peaks_by_pair,
         selection.x_tx,
         selection.x_rx,
         velocity_mps,
-        shell_sigma_m
+        shell_sigma_m,
+        PeakDomain::Corrected
     );
     results.collection_payloads.push_back(build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator));