new GPR

data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp

@@ -44,10 +44,14 @@ struct ProcessingLiveConfig {
     std::vector<std::uint32_t> gpr_output_positions{};
     float gpr_min_depth_m = 2.0F;
     float gpr_max_depth_m = 14.0F;
-    float gpr_range_comp_power = 0.28F;
-    float gpr_angle_comp_power = 0.10F;
+    float gpr_range_comp_power = 0.1F;
+    float gpr_angle_comp_power = 0.0F;
     float gpr_comp_power = 0.2F;
     std::string gpr_score_mode = "combined";
+    // Backprojection intra-sweep speed-correction mode: "int_minus" (full
+    // correction) or "int_focus" (focusing residual only). Mirrors the Python
+    // Horns_motion_3libre.py MOTION_CORRECTION_MODE selector.
+    std::string gpr_motion_mode = "int_minus";
     float gpr_speed_m_s = 0.0F;
     float gpr_look_angle_deg = 0.0F;
     // Motion model parameters for the legacy GPR pipeline. The direction sign

data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp

@@ -60,6 +60,13 @@ using Json = nlohmann::json;
     throw std::runtime_error(field_name + " must be one of: peak, combined");
 }
 
+[[nodiscard]] auto parse_gpr_motion_mode(const std::string& value, const std::string& field_name) -> std::string {
+    if (value == "int_minus" || value == "int_focus") {
+        return value;
+    }
+    throw std::runtime_error(field_name + " must be one of: int_minus, int_focus");
+}
+
 void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) {
     if (value == "backprojection") {
         return;
@@ -266,6 +273,12 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
         }
         config.gpr_score_mode = parse_gpr_score_mode(found->get<std::string>(), "processing.gpr_score_mode");
     }
+    if (const auto found = root.find("gpr_motion_mode"); found != root.end()) {
+        if (!found->is_string()) {
+            throw std::runtime_error("processing.gpr_motion_mode must be string");
+        }
+        config.gpr_motion_mode = parse_gpr_motion_mode(found->get<std::string>(), "processing.gpr_motion_mode");
+    }
     if (const auto found = root.find("gpr_speed_m_s"); found != root.end()) {
         if (!found->is_number()) {
             throw std::runtime_error("processing.gpr_speed_m_s must be number");

data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp

@@ -16,8 +16,12 @@ constexpr double kPairNormPercentile = 50.0;
 constexpr double kPairNormEps = 1e-15;
 
 constexpr double kSmoothSigma = 1.5;
+// Gaussian-kernel half-width in sigmas, matching scipy.ndimage.gaussian_filter's
+// default `truncate=4.0` (radius = int(truncate*sigma + 0.5)). Boundaries use the
+// same default 'reflect' (half-sample symmetric) extension — see reflect_index.
+constexpr double kGaussianTruncate = 4.0;
 constexpr std::size_t kMaxObjects = 10U;
-constexpr double kObjectMinFrac = 0.35;
+constexpr double kObjectMinFrac = 0.7;
 constexpr double kRegionThresholdFrac = 0.75;
 constexpr double kSuppressThresholdFrac = 0.20;
 constexpr double kSuppressRadiusXM = 0.80;
@@ -48,6 +52,16 @@ constexpr double kScoreCfEps = 1e-12;
 
 using PairKey = std::uint64_t;
 
+// Frequency-domain speed correction mode (Horns_motion_3libre.py MOTION_CONFIG):
+//   IntMinus — full intra-frequency correction to the interleaved-sweep centre.
+//   IntFocus — remove the constant + linear-in-frequency part of phi(f), leaving
+//              the focusing residual without a net Z shift.
+enum class MotionMode { IntMinus, IntFocus };
+
+[[nodiscard]] auto parse_motion_mode(const std::string& value) -> MotionMode {
+    return value == "int_focus" ? MotionMode::IntFocus : MotionMode::IntMinus;
+}
+
 struct GeometrySelection {
     // Per-local-index Tx/Rx antenna coordinates in metres. y_/z_ default to 0
     // for legacy configs so the imaging plane coincides with the antennas.
@@ -269,6 +283,25 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     return static_cast<std::size_t>(value);
 }
 
+// scipy 'reflect' boundary (half-sample symmetric): the signal is mirrored about
+// the outer edge of the first/last sample, so index -1 maps to 0, -2 to 1, n to
+// n-1, and so on. Matches scipy.ndimage.gaussian_filter's default mode.
+[[nodiscard]] auto reflect_index(std::ptrdiff_t value, std::size_t limit) -> std::size_t {
+    if (limit <= 1U) {
+        return 0U;
+    }
+    const auto extent = static_cast<std::ptrdiff_t>(limit);
+    const std::ptrdiff_t period = 2 * extent;
+    std::ptrdiff_t wrapped = value % period;
+    if (wrapped < 0) {
+        wrapped += period;
+    }
+    if (wrapped >= extent) {
+        wrapped = (period - 1) - wrapped;
+    }
+    return static_cast<std::size_t>(wrapped);
+}
+
 [[nodiscard]] auto max_value(const std::vector<double>& values) -> double {
     if (values.empty()) {
         return 0.0;
@@ -291,7 +324,7 @@ void normalize_in_place(std::vector<double>& values) {
         return {1.0};
     }
 
-    const auto radius = static_cast<std::ptrdiff_t>(std::ceil(sigma * 3.0));
+    const auto radius = static_cast<std::ptrdiff_t>((kGaussianTruncate * sigma) + 0.5);
     std::vector<double> kernel(static_cast<std::size_t>((radius * 2) + 1), 0.0);
     double sum = 0.0;
     for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) {
@@ -326,7 +359,7 @@ void normalize_in_place(std::vector<double>& values) {
         for (std::size_t col = 0U; col < width; ++col) {
             double sum = 0.0;
             for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) {
-                const auto sample_col = clamp_index(static_cast<std::ptrdiff_t>(col) + offset, width);
+                const auto sample_col = reflect_index(static_cast<std::ptrdiff_t>(col) + offset, width);
                 sum += values[(row * width) + sample_col] * kernel[static_cast<std::size_t>(offset + radius)];
             }
             temp[(row * width) + col] = sum;
@@ -337,7 +370,7 @@ void normalize_in_place(std::vector<double>& values) {
         for (std::size_t col = 0U; col < width; ++col) {
             double sum = 0.0;
             for (std::ptrdiff_t offset = -radius; offset <= radius; ++offset) {
-                const auto sample_row = clamp_index(static_cast<std::ptrdiff_t>(row) + offset, height);
+                const auto sample_row = reflect_index(static_cast<std::ptrdiff_t>(row) + offset, height);
                 sum += temp[(sample_row * width) + col] * kernel[static_cast<std::size_t>(offset + radius)];
             }
             output[(row * width) + col] = sum;
@@ -580,6 +613,114 @@ void validate_collection_trace_order(
     return traces;
 }
 
+// Frequency-domain speed correction applied to S21 *before* the IFFT, ported
+// from Horns_motion_3libre.py (apply_intra_sweep_phase_correction +
+// compute_frequency_sample_times). The measurement is interleaved Tx-by-frequency
+// — Tx0(f0), Tx1(f0), Tx0(f1), ... — so each Tx occupies one interleave slot and
+// its frequency k is sampled at global step (slot_count*k + slot). Each in-band
+// frequency's phase is rotated back to the centre of the (full) interleaved sweep.
+//
+// Unlike the offline Python script, every physical input is taken from the live
+// system: `speed_mps` is the socket-fed velocity, `look_angle_deg`/`direction_sign`
+// are config, and `tx_sweep_time_s` is derived per-collection from the acquisition
+// timestamps (capture_span / Tx count). A zero speed or non-positive sweep time
+// makes this a no-op.
+void apply_intra_sweep_motion_correction(
+    SelectedTrace& trace,
+    std::size_t slot,
+    std::size_t slot_count,
+    double start_hz,
+    double stop_hz,
+    double velocity_mps,
+    double speed_mps,
+    double look_angle_deg,
+    double direction_sign,
+    double tx_sweep_time_s,
+    MotionMode motion_mode
+) {
+    const std::size_t n_full = trace.frequency_hz.size();
+    if (n_full < 2U || trace.s21.size() != n_full || slot_count == 0U
+        || !(velocity_mps > 0.0) || !(tx_sweep_time_s > 0.0)) {
+        return;
+    }
+
+    const double low_hz = std::min(start_hz, stop_hz);
+    const double high_hz = std::max(start_hz, stop_hz);
+
+    // Absolute sample time of full-sweep frequency index k for this Tx slot.
+    const double dt_base = tx_sweep_time_s / static_cast<double>(n_full - 1U);
+    const auto sample_time_s = [&](std::size_t index) {
+        return ((static_cast<double>(slot_count) * static_cast<double>(index)) + static_cast<double>(slot)) * dt_base;
+    };
+    // t_center is the midpoint of the *full* interleaved sweep for this slot,
+    // i.e. between the first and last full-array frequencies (not the cut band).
+    const double t_center_s = 0.5 * (sample_time_s(0U) + sample_time_s(n_full - 1U));
+
+    const double theta_rad = (look_angle_deg * kPi) / 180.0;
+    const double motion_factor = direction_sign * speed_mps * std::cos(theta_rad);
+
+    std::vector<std::size_t> band_indices{};
+    std::vector<double> band_frequency_hz{};
+    std::vector<double> phi{};
+    band_indices.reserve(n_full);
+    band_frequency_hz.reserve(n_full);
+    phi.reserve(n_full);
+
+    bool any_nonzero = false;
+    for (std::size_t index = 0U; index < n_full; ++index) {
+        const double frequency = trace.frequency_hz[index];
+        if (frequency < low_hz || frequency > high_hz) {
+            continue;
+        }
+        const double dt_intra_s = sample_time_s(index) - t_center_s;
+        const double delta_range_m = motion_factor * dt_intra_s;
+        const double delta_path_m = 2.0 * delta_range_m;
+        const double dtau_s = delta_path_m / velocity_mps;
+        if (dtau_s != 0.0) {
+            any_nonzero = true;
+        }
+        band_indices.push_back(index);
+        band_frequency_hz.push_back(frequency);
+        phi.push_back(2.0 * kPi * frequency * dtau_s);
+    }
+
+    if (!any_nonzero || band_indices.empty()) {
+        return;
+    }
+
+    // int_focus removes the constant + linear-in-frequency component of phi(f).
+    // With f_rel = f - mean(f) the design columns [1, f_rel] are orthogonal, so
+    // the least-squares fit is intercept = mean(phi), slope = <f_rel, phi>/<f_rel, f_rel>.
+    if (motion_mode == MotionMode::IntFocus) {
+        const auto count = static_cast<double>(band_frequency_hz.size());
+        double mean_frequency = 0.0;
+        double mean_phi = 0.0;
+        for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
+            mean_frequency += band_frequency_hz[i];
+            mean_phi += phi[i];
+        }
+        mean_frequency /= count;
+        mean_phi /= count;
+
+        double cross = 0.0;
+        double f_rel_sq = 0.0;
+        for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
+            const double f_rel = band_frequency_hz[i] - mean_frequency;
+            cross += f_rel * phi[i];
+            f_rel_sq += f_rel * f_rel;
+        }
+        const double slope = (f_rel_sq > 0.0) ? (cross / f_rel_sq) : 0.0;
+        for (std::size_t i = 0U; i < phi.size(); ++i) {
+            const double f_rel = band_frequency_hz[i] - mean_frequency;
+            phi[i] -= mean_phi + (slope * f_rel);
+        }
+    }
+
+    for (std::size_t i = 0U; i < band_indices.size(); ++i) {
+        trace.s21[band_indices[i]] *= std::polar(1.0, -phi[i]);
+    }
+}
+
 [[nodiscard]] auto compute_ascan(
     const SelectedTrace& trace,
     double start_hz,
@@ -841,6 +982,38 @@ void normalize_pair_ascans(
     return std::clamp(range_weight * angle_weight, 0.0, kTotalWeightMax);
 }
 
+// Run `body(row_begin, row_end)` over a partition of [0, row_count) across the
+// available hardware threads. Each call owns a disjoint, contiguous row range, so
+// a body that writes only its own rows needs no synchronization. The calling
+// thread runs the first chunk while spawned workers handle the rest. Falls back to
+// a single serial call when there is one row or no concurrency is reported.
+template <typename Body>
+void parallel_for_rows(std::size_t row_count, const Body& body) {
+    if (row_count == 0U) {
+        return;
+    }
+
+    const unsigned int detected = std::thread::hardware_concurrency();
+    const std::size_t worker_count = std::clamp<std::size_t>(
+        detected == 0U ? 1U : static_cast<std::size_t>(detected), 1U, row_count
+    );
+    if (worker_count == 1U) {
+        body(0U, row_count);
+        return;
+    }
+
+    const std::size_t chunk = (row_count + worker_count - 1U) / worker_count;
+    std::vector<std::thread> workers;
+    workers.reserve(worker_count - 1U);
+    for (std::size_t begin = chunk; begin < row_count; begin += chunk) {
+        workers.emplace_back(body, begin, std::min(begin + chunk, row_count));
+    }
+    body(0U, std::min(chunk, row_count));
+    for (auto& worker : workers) {
+        worker.join();
+    }
+}
+
 [[nodiscard]] auto backproject_coherent(
     const std::vector<SelectedTrace>& selected_traces,
     const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
@@ -862,77 +1035,102 @@ void normalize_pair_ascans(
     result.coherent.assign(cell_count, std::complex<double>(0.0, 0.0));
     result.incoherent.assign(cell_count, 0.0);
     result.coherence_factor.assign(cell_count, 0.0);
-    std::vector<double> contribution_count(cell_count, 0.0);
 
+    // Resolve each contributing pair once, in selected-trace order. Iterating
+    // these per cell reproduces the serial accumulation order exactly, so the
+    // parallel result is bit-for-bit identical to a single-threaded sweep.
+    struct PairContribution {
+        const std::vector<double>* tx_distances;
+        const std::vector<double>* rx_distances;
+        const AscanResult* ascan;
+        double geo_ref;
+        double angle_ref;
+        double z_tx_ant;
+        double z_rx_ant;
+    };
+
+    std::vector<PairContribution> contributions;
+    contributions.reserve(selected_traces.size());
     for (const auto& trace : selected_traces) {
         const auto key = make_pair_key(trace.tx_local_index, trace.rx_local_index);
         const auto ascan_it = ascans_by_pair.find(key);
-        if (ascan_it == ascans_by_pair.end()) {
+        if (ascan_it == ascans_by_pair.end() || ascan_it->second.time_s.empty()) {
             continue;
         }
-        const auto& ascan = ascan_it->second;
-        if (ascan.time_s.empty()) {
-            continue;
-        }
-
         const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth(
             trace.tx_local_index,
             trace.rx_local_index,
             selection,
             imaging_plane_y_m
         );
-        const auto& tx_distances = grid.tx_distance_grids[trace.tx_local_index];
-        const auto& rx_distances = grid.rx_distance_grids[trace.rx_local_index];
-        const double z_tx_ant = selection.z_tx[trace.tx_local_index];
-        const double z_rx_ant = selection.z_rx[trace.rx_local_index];
+        contributions.push_back(PairContribution{
+            &grid.tx_distance_grids[trace.tx_local_index],
+            &grid.rx_distance_grids[trace.rx_local_index],
+            &ascan_it->second,
+            geo_ref,
+            angle_ref,
+            selection.z_tx[trace.tx_local_index],
+            selection.z_rx[trace.rx_local_index],
+        });
+    }
 
-        for (std::size_t row = 0U; row < height; ++row) {
+    // Each grid row writes only its own cells, so rows partition cleanly across
+    // threads with no shared mutable state. Within a cell the contributions are
+    // summed in pair order and then averaged — the same arithmetic, in the same
+    // order, as the original serial pair-outer/cell-inner loop.
+    const auto accumulate_rows = [&](std::size_t row_begin, std::size_t row_end) {
+        for (std::size_t row = row_begin; row < row_end; ++row) {
             const double z_m = grid.z_grid[row];
-            const bool in_depth_gate = z_m >= min_depth_m && z_m <= max_depth_m;
-            if (!in_depth_gate) {
-                continue;
+            if (z_m < min_depth_m || z_m > max_depth_m) {
+                continue;  // Depth-gated rows stay zero, as in the serial version.
             }
-            const double dz_tx = z_m - z_tx_ant;
-            const double dz_rx = z_m - z_rx_ant;
 
             for (std::size_t col = 0U; col < width; ++col) {
                 const auto cell_index = (row * width) + col;
-                const double r_tx = tx_distances[cell_index];
-                const double r_rx = rx_distances[cell_index];
-                const double tau_s = (r_tx + r_rx) / velocity_mps;
-                if (tau_s < ascan.time_s.front() || tau_s > ascan.time_s.back()) {
+                std::complex<double> coherent_sum(0.0, 0.0);
+                double incoherent_sum = 0.0;
+                double contribution_count = 0.0;
+
+                for (const auto& contribution : contributions) {
+                    const double r_tx = (*contribution.tx_distances)[cell_index];
+                    const double r_rx = (*contribution.rx_distances)[cell_index];
+                    const double tau_s = (r_tx + r_rx) / velocity_mps;
+                    const auto& ascan = *contribution.ascan;
+                    if (tau_s < ascan.time_s.front() || tau_s > ascan.time_s.back()) {
+                        continue;
+                    }
+
+                    const auto sample = interpolate_complex(ascan, tau_s);
+                    const double weight = compensation_weight(
+                        r_tx,
+                        r_rx,
+                        z_m - contribution.z_tx_ant,
+                        z_m - contribution.z_rx_ant,
+                        contribution.geo_ref,
+                        contribution.angle_ref,
+                        range_power,
+                        angle_power
+                    );
+                    coherent_sum += sample * weight;
+                    incoherent_sum += std::abs(sample) * weight;
+                    contribution_count += 1.0;
+                }
+
+                if (!(contribution_count > 0.0)) {
                     continue;
                 }
-                const auto sample = interpolate_complex(ascan, tau_s);
-
-                const double weight = compensation_weight(
-                    r_tx,
-                    r_rx,
-                    dz_tx,
-                    dz_rx,
-                    geo_ref,
-                    angle_ref,
-                    range_power,
-                    angle_power
-                );
-                result.coherent[cell_index] += sample * weight;
-                result.incoherent[cell_index] += std::abs(sample) * weight;
-                contribution_count[cell_index] += 1.0;
+                coherent_sum /= contribution_count;
+                incoherent_sum /= contribution_count;
+                result.coherent[cell_index] = coherent_sum;
+                result.incoherent[cell_index] = incoherent_sum;
+                result.image[cell_index] = std::abs(coherent_sum);
+                result.coherence_factor[cell_index] =
+                    std::clamp(result.image[cell_index] / (incoherent_sum + kScoreCfEps), 0.0, 1.0);
             }
         }
-    }
-
-    for (std::size_t index = 0U; index < cell_count; ++index) {
-        if (!(contribution_count[index] > 0.0)) {
-            continue;
-        }
-        result.coherent[index] /= contribution_count[index];
-        result.incoherent[index] /= contribution_count[index];
-        result.image[index] = std::abs(result.coherent[index]);
-        result.coherence_factor[index] =
-            std::clamp(result.image[index] / (result.incoherent[index] + kScoreCfEps), 0.0, 1.0);
-    }
+    };
 
+    parallel_for_rows(height, accumulate_rows);
     return result;
 }
 
@@ -1595,7 +1793,7 @@ void add_bp_score_metrics(
 
     validate_collection_trace_order(run_config, collection);
     const auto background_mean = build_background_mean(previous_collections, selection, live_config);
-    const auto selected_traces = collect_selected_traces(collection, selection, background_mean);
+    auto selected_traces = collect_selected_traces(collection, selection, background_mean);
     if (selected_traces.empty()) {
         return results;
     }
@@ -1610,6 +1808,38 @@ void add_bp_score_metrics(
         return results;
     }
 
+    // Intra-sweep speed correction before the IFFT. The Tx interleave slot is the
+    // Tx local index (outputs are sorted), with slot_count = number of selected Tx.
+    // tx_sweep_time is derived per-collection from the acquisition timestamps:
+    // one Tx's interleaved sweep spans the whole frame, so capture_span / N_tx.
+    // Speed comes from the socket-fed live config; look-angle/direction from config.
+    const auto motion_mode = parse_motion_mode(live_config.gpr_motion_mode);
+    const std::size_t motion_slot_count = selection.output_positions.size();
+    const double capture_span_s =
+        collection.capture_end_ns > collection.capture_start_ns
+            ? static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9
+            : 0.0;
+    const double tx_sweep_time_s =
+        motion_slot_count > 0U ? capture_span_s / static_cast<double>(motion_slot_count) : 0.0;
+    const double motion_speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
+    const double motion_look_angle_deg = static_cast<double>(live_config.gpr_look_angle_deg);
+    const double motion_direction_sign = static_cast<double>(live_config.gpr_direction_sign);
+    for (auto& trace : selected_traces) {
+        apply_intra_sweep_motion_correction(
+            trace,
+            trace.tx_local_index,
+            motion_slot_count,
+            start_hz,
+            stop_hz,
+            velocity_mps,
+            motion_speed_mps,
+            motion_look_angle_deg,
+            motion_direction_sign,
+            tx_sweep_time_s,
+            motion_mode
+        );
+    }
+
     std::unordered_map<PairKey, AscanResult> ascans_by_pair{};
     for (const auto& trace : selected_traces) {
         auto ascan = compute_ascan(trace, start_hz, stop_hz);

data_acq_and_processing/processing/processors/src/gpr_processor.cpp

@@ -10,6 +10,7 @@
 #include <queue>
 #include <stdexcept>
 #include <string>
+#include <thread>
 #include <tuple>
 #include <unordered_map>
 #include <unordered_set>