kamil_adc support added

data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp

@@ -9,7 +9,11 @@ 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 kCompensationReferenceDepthM = 5.0;
+
+constexpr bool kPairNormalize = true;
+constexpr double kPairNormPercentile = 50.0;
+constexpr double kPairNormEps = 1e-15;
 
 constexpr double kSmoothSigma = 1.5;
 constexpr std::size_t kMaxObjects = 10U;
@@ -30,6 +34,18 @@ constexpr double kSidelobeMinDxM = 0.35;
 constexpr double kSidelobeMaxDzM = 0.70;
 constexpr double kSidelobeMaxRelativePeak = 0.85;
 
+constexpr double kLocalBgRadiusXM = 1.20;
+constexpr double kLocalBgRadiusZM = 0.80;
+constexpr double kLocalBgPercentile = 50.0;
+constexpr double kLocalContrastEps = 1e-12;
+
+constexpr double kScoreCohPeakWeight = 0.45;
+constexpr double kScoreCoherenceFactorWeight = 0.25;
+constexpr double kScoreProminenceWeight = 0.20;
+constexpr double kScoreContrastWeight = 0.10;
+constexpr double kScoreContrastCap = 6.0;
+constexpr double kScoreCfEps = 1e-12;
+
 using PairKey = std::uint64_t;
 
 struct GeometrySelection {
@@ -72,10 +88,13 @@ struct GridDefinition {
 struct BpMap {
     std::vector<double> image{};
     std::vector<std::complex<double>> coherent{};
+    std::vector<double> incoherent{};
+    std::vector<double> coherence_factor{};
 };
 
 struct ObjectRecord {
     std::size_t index = 0U;
+    std::size_t peak_index = 0U;
     double x_peak_m = 0.0;
     double z_peak_m = 0.0;
     double x_m = 0.0;
@@ -87,6 +106,18 @@ struct ObjectRecord {
     double center_area_cm2 = 0.0;
     double mean_value = 0.0;
     double sum_value = 0.0;
+    double local_bg = 0.0;
+    double local_bg_p75 = 0.0;
+    double prominence = 0.0;
+    double contrast = 0.0;
+    double incoh_peak = 0.0;
+    double incoh_mean = 0.0;
+    double incoh_center_mean = 0.0;
+    double coherence_factor_peak = 0.0;
+    double coherence_factor_center = 0.0;
+    double score_old = 0.0;
+    double score_new = 0.0;
+    double selected_score = 0.0;
     std::vector<std::uint8_t> region_mask{};
     std::vector<std::uint8_t> center_mask{};
     bool sidelobe_candidate = false;
@@ -178,6 +209,28 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
     return median;
 }
 
+[[nodiscard]] auto percentile_copy(std::vector<double> values, double percentile) -> double {
+    if (values.empty()) {
+        return 0.0;
+    }
+
+    values.erase(
+        std::remove_if(values.begin(), values.end(), [](double value) { return !std::isfinite(value); }),
+        values.end()
+    );
+    if (values.empty()) {
+        return 0.0;
+    }
+
+    std::sort(values.begin(), values.end());
+    const double clamped_percentile = std::clamp(percentile, 0.0, 100.0);
+    const double position = (clamped_percentile / 100.0) * static_cast<double>(values.size() - 1U);
+    const auto lower_index = static_cast<std::size_t>(std::floor(position));
+    const auto upper_index = std::min<std::size_t>(lower_index + 1U, values.size() - 1U);
+    const double fraction = position - static_cast<double>(lower_index);
+    return (values[lower_index] * (1.0 - fraction)) + (values[upper_index] * fraction);
+}
+
 [[nodiscard]] auto build_axis(double min_value, double max_value, std::size_t count) -> std::vector<double> {
     std::vector<double> axis{};
     if (count == 0U) {
@@ -576,6 +629,50 @@ void validate_collection_trace_order(
     return result;
 }
 
+void normalize_pair_ascans(
+    std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
+    double velocity_mps,
+    double min_depth_m,
+    double max_depth_m
+) {
+    if (!kPairNormalize) {
+        return;
+    }
+
+    for (auto& item : ascans_by_pair) {
+        auto& ascan = item.second;
+        if (ascan.samples.empty() || ascan.time_s.size() != ascan.samples.size()) {
+            continue;
+        }
+
+        std::vector<double> amplitudes{};
+        amplitudes.reserve(ascan.samples.size());
+        for (std::size_t index = 0U; index < ascan.samples.size(); ++index) {
+            const double depth_m = 0.5 * velocity_mps * ascan.time_s[index];
+            if (depth_m < min_depth_m || depth_m > max_depth_m) {
+                continue;
+            }
+            amplitudes.push_back(std::abs(ascan.samples[index]));
+        }
+        if (amplitudes.empty()) {
+            amplitudes.reserve(ascan.samples.size());
+            for (const auto& sample : ascan.samples) {
+                amplitudes.push_back(std::abs(sample));
+            }
+        }
+
+        double scale = percentile_copy(std::move(amplitudes), kPairNormPercentile);
+        if (!std::isfinite(scale) || !(scale > kPairNormEps)) {
+            scale = 1.0;
+        }
+
+        const double denominator = scale + kPairNormEps;
+        for (auto& sample : ascan.samples) {
+            sample /= denominator;
+        }
+    }
+}
+
 [[nodiscard]] auto build_grid(
     const std::vector<double>& x_tx,
     const std::vector<double>& x_rx,
@@ -706,6 +803,8 @@ void validate_collection_trace_order(
     BpMap result{};
     result.image.assign(cell_count, 0.0);
     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);
 
     for (const auto& trace : selected_traces) {
@@ -714,6 +813,10 @@ void validate_collection_trace_order(
         if (ascan_it == ascans_by_pair.end()) {
             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,
@@ -736,10 +839,10 @@ void validate_collection_trace_order(
                 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<double>(0.0, 0.0)) {
+                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,
@@ -751,6 +854,7 @@ void validate_collection_trace_order(
                     angle_power
                 );
                 result.coherent[cell_index] += sample * weight;
+                result.incoherent[cell_index] += std::abs(sample) * weight;
                 contribution_count[cell_index] += 1.0;
             }
         }
@@ -761,7 +865,10 @@ void validate_collection_trace_order(
             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);
     }
 
     return result;
@@ -786,6 +893,21 @@ void apply_depth_gate(
     }
 }
 
+[[nodiscard]] auto normalize_bp_map(
+    const std::vector<double>& raw_image,
+    const GridDefinition& grid,
+    double min_depth_m,
+    double max_depth_m,
+    double smooth_sigma
+) -> std::vector<double> {
+    auto normalized = raw_image;
+    normalize_in_place(normalized);
+    normalized = gaussian_filter_2d(normalized, grid.x_grid.size(), grid.z_grid.size(), smooth_sigma);
+    apply_depth_gate(normalized, grid.z_grid, grid.x_grid.size(), min_depth_m, max_depth_m);
+    normalize_in_place(normalized);
+    return normalized;
+}
+
 [[nodiscard]] auto neighbor_indices(
     std::size_t index,
     std::size_t width,
@@ -1045,6 +1167,7 @@ void apply_depth_gate(
 
         ObjectRecord object{};
         object.index = objects.size() + 1U;
+        object.peak_index = peak_index;
         object.x_peak_m = x_peak;
         object.z_peak_m = z_peak;
         object.x_m = x_center;
@@ -1153,6 +1276,187 @@ void mark_sidelobe_candidates(
     }
 }
 
+void add_local_prominence_metrics(
+    std::vector<ObjectRecord>& objects,
+    const std::vector<double>& bp_image,
+    const GridDefinition& grid,
+    double min_depth_m,
+    double max_depth_m
+) {
+    const std::size_t width = grid.x_grid.size();
+    if (bp_image.empty() || width == 0U) {
+        return;
+    }
+
+    for (auto& object : objects) {
+        std::vector<double> bg_values{};
+        for (std::size_t index = 0U; index < bp_image.size(); ++index) {
+            const auto row = index / width;
+            const auto col = index % width;
+            const double x_m = grid.x_grid[col];
+            const double z_m = grid.z_grid[row];
+            const bool in_local_window =
+                std::abs(x_m - object.x_peak_m) <= kLocalBgRadiusXM &&
+                std::abs(z_m - object.z_peak_m) <= kLocalBgRadiusZM;
+            const bool in_depth_gate = z_m >= min_depth_m && z_m <= max_depth_m;
+            if (in_local_window && in_depth_gate && object.region_mask[index] == 0U) {
+                bg_values.push_back(bp_image[index]);
+            }
+        }
+
+        if (bg_values.size() < 10U) {
+            bg_values.clear();
+            for (std::size_t index = 0U; index < bp_image.size(); ++index) {
+                const auto row = index / width;
+                const double z_m = grid.z_grid[row];
+                if (z_m >= min_depth_m && z_m <= max_depth_m && object.region_mask[index] == 0U) {
+                    bg_values.push_back(bp_image[index]);
+                }
+            }
+        }
+
+        object.local_bg = percentile_copy(bg_values, kLocalBgPercentile);
+        object.local_bg_p75 = percentile_copy(std::move(bg_values), 75.0);
+        object.prominence = object.peak - object.local_bg;
+        object.contrast = object.peak / (object.local_bg + kLocalContrastEps);
+    }
+}
+
+[[nodiscard]] auto mean_values_under_mask(
+    const std::vector<double>& values,
+    const std::vector<std::uint8_t>& mask
+) -> double {
+    if (values.empty() || values.size() != mask.size()) {
+        return 0.0;
+    }
+
+    double sum = 0.0;
+    std::size_t count = 0U;
+    for (std::size_t index = 0U; index < values.size(); ++index) {
+        if (mask[index] == 0U) {
+            continue;
+        }
+        sum += values[index];
+        count += 1U;
+    }
+    return count > 0U ? sum / static_cast<double>(count) : 0.0;
+}
+
+[[nodiscard]] auto max_values_under_mask(
+    const std::vector<double>& values,
+    const std::vector<std::uint8_t>& mask
+) -> double {
+    if (values.empty() || values.size() != mask.size()) {
+        return 0.0;
+    }
+
+    double maximum = 0.0;
+    bool has_value = false;
+    for (std::size_t index = 0U; index < values.size(); ++index) {
+        if (mask[index] == 0U) {
+            continue;
+        }
+        maximum = has_value ? std::max(maximum, values[index]) : values[index];
+        has_value = true;
+    }
+    return has_value ? maximum : 0.0;
+}
+
+void add_incoherent_support_metrics(
+    std::vector<ObjectRecord>& objects,
+    const std::vector<double>& bp_incoherent_image,
+    const std::vector<double>& bp_coherence_factor
+) {
+    for (auto& object : objects) {
+        object.incoh_peak = max_values_under_mask(bp_incoherent_image, object.region_mask);
+        object.incoh_mean = mean_values_under_mask(bp_incoherent_image, object.region_mask);
+        object.incoh_center_mean = mean_values_under_mask(bp_incoherent_image, object.center_mask);
+        if (!(object.incoh_center_mean > 0.0)) {
+            object.incoh_center_mean = object.incoh_mean;
+        }
+
+        if (!bp_coherence_factor.empty() && object.peak_index < bp_coherence_factor.size()) {
+            object.coherence_factor_peak = std::clamp(bp_coherence_factor[object.peak_index], 0.0, 1.0);
+        } else {
+            object.coherence_factor_peak =
+                std::clamp(object.peak / (object.incoh_peak + kScoreCfEps), 0.0, 1.0);
+        }
+
+        const double center_cf = mean_values_under_mask(bp_coherence_factor, object.center_mask);
+        object.coherence_factor_center = std::clamp(
+            center_cf > 0.0
+                ? center_cf
+                : object.mean_value / (object.incoh_center_mean + kScoreCfEps),
+            0.0,
+            1.0
+        );
+    }
+}
+
+[[nodiscard]] auto contrast_score_unit(double contrast) -> double {
+    if (!std::isfinite(contrast) || kScoreContrastCap <= 1.0) {
+        return 0.0;
+    }
+    return std::clamp((contrast - 1.0) / (kScoreContrastCap - 1.0), 0.0, 1.0);
+}
+
+[[nodiscard]] auto use_combined_gpr_score(const ProcessingLiveConfig& live_config) -> bool {
+    return live_config.gpr_score_mode == "combined";
+}
+
+void add_bp_score_metrics(
+    std::vector<ObjectRecord>& objects,
+    const ProcessingLiveConfig& live_config
+) {
+    double total_weight =
+        kScoreCohPeakWeight +
+        kScoreCoherenceFactorWeight +
+        kScoreProminenceWeight +
+        kScoreContrastWeight;
+    if (!(total_weight > 0.0)) {
+        total_weight = 1.0;
+    }
+
+    const bool combined_score = use_combined_gpr_score(live_config);
+    for (auto& object : objects) {
+        const double coh_peak_score = std::clamp(object.peak, 0.0, 1.0);
+        const double coherence_factor_score = std::clamp(object.coherence_factor_peak, 0.0, 1.0);
+        const double prominence_score = std::clamp(object.prominence, 0.0, 1.0);
+        const double contrast_score = contrast_score_unit(object.contrast);
+
+        object.score_old = coh_peak_score;
+        object.score_new = (
+            (kScoreCohPeakWeight * coh_peak_score) +
+            (kScoreCoherenceFactorWeight * coherence_factor_score) +
+            (kScoreProminenceWeight * prominence_score) +
+            (kScoreContrastWeight * contrast_score)
+        ) / total_weight;
+        object.selected_score = combined_score ? object.score_new : object.score_old;
+    }
+}
+
+[[nodiscard]] auto output_objects_sorted(
+    const std::vector<ObjectRecord>& objects,
+    const ProcessingLiveConfig& live_config
+) -> std::vector<const ObjectRecord*> {
+    std::vector<const ObjectRecord*> visible{};
+    visible.reserve(objects.size());
+    for (const auto& object : objects) {
+        if (live_config.gpr_remove_sidelobe_objects_enabled && object.sidelobe_candidate) {
+            continue;
+        }
+        visible.push_back(&object);
+    }
+
+    std::sort(visible.begin(), visible.end(), [](const ObjectRecord* left, const ObjectRecord* right) {
+        if (left->selected_score == right->selected_score) {
+            return left->peak > right->peak;
+        }
+        return left->selected_score > right->selected_score;
+    });
+    return visible;
+}
+
 [[nodiscard]] auto flatten_table(
     const std::vector<std::vector<float>>& rows,
     std::uint32_t column_count
@@ -1248,6 +1552,7 @@ void mark_sidelobe_candidates(
     if (ascans_by_pair.empty()) {
         return results;
     }
+    normalize_pair_ascans(ascans_by_pair, velocity_mps, min_depth_m, max_depth_m);
 
     const auto grid = build_grid(selection.x_tx, selection.x_rx, max_depth_m, kGridZMinM);
     if (grid.x_grid.empty() || grid.z_grid.empty()) {
@@ -1270,13 +1575,15 @@ void mark_sidelobe_candidates(
         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 display_map = normalize_bp_map(bp.image, grid, min_depth_m, max_depth_m, kSmoothSigma);
+    const auto incoherent_display_map =
+        normalize_bp_map(bp.incoherent, grid, min_depth_m, max_depth_m, kSmoothSigma);
 
     auto objects = find_bp_objects(display_map, grid);
+    add_local_prominence_metrics(objects, display_map, grid, min_depth_m, max_depth_m);
+    add_incoherent_support_metrics(objects, incoherent_display_map, bp.coherence_factor);
     mark_sidelobe_candidates(objects, selected_traces, selection.x_tx, selection.x_rx);
+    add_bp_score_metrics(objects, live_config);
 
     if (live_config.gpr_remove_sidelobe_objects_enabled) {
         for (const auto& object : objects) {
@@ -1295,15 +1602,12 @@ void mark_sidelobe_candidates(
 
     std::vector<std::vector<float>> point_rows{};
     point_rows.reserve(objects.size());
-    for (const auto& object : objects) {
-        if (live_config.gpr_remove_sidelobe_objects_enabled && object.sidelobe_candidate) {
-            continue;
-        }
+    for (const auto* object : output_objects_sorted(objects, live_config)) {
         point_rows.push_back(
             {
-                static_cast<float>(object.x_m),
-                static_cast<float>(object.z_m),
-                static_cast<float>(object.peak),
+                static_cast<float>(object->x_m),
+                static_cast<float>(object->z_m),
+                static_cast<float>(object->selected_score),
             }
         );
     }