added legacy gpr
This commit is contained in:
Ayzen
@@ -13,6 +13,12 @@ enum class HistoryCommand {
|
||||
ClearAll,
|
||||
};
|
||||
|
||||
enum class GprAlgorithm {
|
||||
Backprojection,
|
||||
LegacyPoint,
|
||||
LegacyExtended,
|
||||
};
|
||||
|
||||
struct ProcessingLiveConfig {
|
||||
std::string processor_mode = "pass_through";
|
||||
std::string pass_through_channel = "s21";
|
||||
@@ -26,12 +32,18 @@ struct ProcessingLiveConfig {
|
||||
float bscan_gain = 1.0F;
|
||||
float bscan_start_freq_mhz = 100.0F;
|
||||
float bscan_stop_freq_mhz = 8800.0F;
|
||||
GprAlgorithm gpr_algorithm = GprAlgorithm::Backprojection;
|
||||
std::vector<std::uint32_t> gpr_input_positions{};
|
||||
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_comp_power = 0.2F;
|
||||
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;
|
||||
float gpr_start_freq_mhz = 3000.0F;
|
||||
float gpr_stop_freq_mhz = 6000.0F;
|
||||
bool gpr_background_subtract_enabled = true;
|
||||
|
||||
@@ -31,6 +31,21 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error("processing.history_command must be one of: none, remove_last, clear_all");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_algorithm(const std::string& value) -> GprAlgorithm {
|
||||
if (value == "backprojection") {
|
||||
return GprAlgorithm::Backprojection;
|
||||
}
|
||||
if (value == "legacy_point") {
|
||||
return GprAlgorithm::LegacyPoint;
|
||||
}
|
||||
if (value == "legacy_extended") {
|
||||
return GprAlgorithm::LegacyExtended;
|
||||
}
|
||||
throw std::runtime_error(
|
||||
"processing.gpr_algorithm must be one of: backprojection, legacy_point, legacy_extended"
|
||||
);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_s_parameter_channel(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "s21" || value == "s11") {
|
||||
return value;
|
||||
@@ -164,6 +179,12 @@ using Json = nlohmann::json;
|
||||
}
|
||||
config.bscan_stop_freq_mhz = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_algorithm"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_algorithm must be string");
|
||||
}
|
||||
config.gpr_algorithm = parse_gpr_algorithm(found->get<std::string>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_input_positions"); found != root.end()) {
|
||||
config.gpr_input_positions = parse_u32_array(*found, "processing.gpr_input_positions");
|
||||
}
|
||||
@@ -194,6 +215,36 @@ using Json = nlohmann::json;
|
||||
}
|
||||
config.gpr_angle_comp_power = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_comp_power"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_comp_power must be number");
|
||||
}
|
||||
config.gpr_comp_power = static_cast<float>(found->get<double>());
|
||||
}
|
||||
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<float>(found->get<double>());
|
||||
}
|
||||
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<float>(found->get<double>());
|
||||
}
|
||||
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<float>(found->get<double>());
|
||||
}
|
||||
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<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_start_freq_mhz"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_start_freq_mhz must be number");
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,827 @@
|
||||
constexpr double kLegacyGridZMinM = 0.20;
|
||||
constexpr double kLegacySmoothSigma = 3.0;
|
||||
constexpr double kLegacyCleanSuppressRadiusM = 0.07;
|
||||
constexpr double kLegacyCleanThresholdFrac = 0.05;
|
||||
constexpr std::size_t kLegacyMaxObjects = 15U;
|
||||
constexpr double kLegacyExtendedThresholdFrac = 0.75;
|
||||
constexpr double kLegacyExtendedMinAreaCm2 = 2.0;
|
||||
|
||||
struct LegacyAscanResult {
|
||||
std::vector<double> time_s{};
|
||||
std::vector<double> depth_m{};
|
||||
std::vector<double> amplitude{};
|
||||
double bandwidth_hz = 0.0;
|
||||
};
|
||||
|
||||
struct LegacyPeakRecord {
|
||||
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 LegacyPointRecord {
|
||||
double x_m = 0.0;
|
||||
double z_m = 0.0;
|
||||
double score = 0.0;
|
||||
};
|
||||
|
||||
struct LegacyRegionRecord {
|
||||
double x_m = 0.0;
|
||||
double z_m = 0.0;
|
||||
double score = 0.0;
|
||||
double pixel_count = 0.0;
|
||||
std::vector<float> mask{};
|
||||
};
|
||||
|
||||
struct LegacyPairTiming {
|
||||
double dtau_motion_s = 0.0;
|
||||
};
|
||||
|
||||
enum class LegacyPeakDomain {
|
||||
Apparent,
|
||||
Corrected,
|
||||
};
|
||||
|
||||
[[nodiscard]] auto find_legacy_peak_indices(
|
||||
const std::vector<double>& values,
|
||||
std::size_t start_index,
|
||||
std::size_t stop_index,
|
||||
double threshold,
|
||||
std::size_t min_distance
|
||||
) -> std::vector<std::size_t> {
|
||||
if (stop_index <= start_index + 2U) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<std::size_t> candidates{};
|
||||
for (std::size_t index = start_index + 1U; index + 1U < stop_index; ++index) {
|
||||
const bool below_threshold = values[index] < threshold;
|
||||
const bool not_rising = values[index] <= values[index - 1U];
|
||||
const bool still_rising = values[index] < values[index + 1U];
|
||||
if (below_threshold || not_rising || still_rising) {
|
||||
continue;
|
||||
}
|
||||
candidates.push_back(index);
|
||||
}
|
||||
|
||||
std::sort(candidates.begin(), candidates.end(), [&](std::size_t left, std::size_t right) {
|
||||
return values[left] > values[right];
|
||||
});
|
||||
|
||||
std::vector<std::size_t> 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::sort(selected.begin(), selected.end());
|
||||
return selected;
|
||||
}
|
||||
|
||||
[[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
|
||||
) -> 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 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);
|
||||
return (geo * pattern) + 1e-30;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto lower_bound_index(const std::vector<double>& axis, double value) -> std::size_t {
|
||||
const auto found = std::lower_bound(axis.begin(), axis.end(), value);
|
||||
return static_cast<std::size_t>(std::distance(axis.begin(), found));
|
||||
}
|
||||
|
||||
[[nodiscard]] auto compute_legacy_ascan(
|
||||
const SelectedTrace& trace,
|
||||
double start_hz,
|
||||
double stop_hz,
|
||||
double velocity_mps
|
||||
) -> LegacyAscanResult {
|
||||
LegacyAscanResult 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<double> frequency_hz{};
|
||||
std::vector<std::complex<double>> s21{};
|
||||
frequency_hz.reserve(trace.frequency_hz.size());
|
||||
s21.reserve(trace.s21.size());
|
||||
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) {
|
||||
continue;
|
||||
}
|
||||
frequency_hz.push_back(frequency_value);
|
||||
s21.push_back(trace.s21[index]);
|
||||
}
|
||||
|
||||
if (frequency_hz.size() < 2U) {
|
||||
return result;
|
||||
}
|
||||
|
||||
const std::size_t point_count = frequency_hz.size();
|
||||
const double df_hz = (frequency_hz.back() - frequency_hz.front()) / static_cast<double>(point_count - 1U);
|
||||
if (!(df_hz > 0.0)) {
|
||||
return result;
|
||||
}
|
||||
|
||||
const auto start_bin = static_cast<std::int64_t>(std::llround(frequency_hz.front() / df_hz));
|
||||
if (start_bin < 0) {
|
||||
return result;
|
||||
}
|
||||
|
||||
const auto start_index = static_cast<std::size_t>(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)) {
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<std::complex<double>> spectrum(fft_len, std::complex<double>(0.0, 0.0));
|
||||
for (std::size_t index = 0U; index < point_count; ++index) {
|
||||
const double window = point_count > 1U
|
||||
? 0.5 - (0.5 * std::cos((2.0 * kPi * static_cast<double>(index)) / static_cast<double>(point_count - 1U)))
|
||||
: 1.0;
|
||||
spectrum[start_index + index] = s21[index] * window;
|
||||
}
|
||||
|
||||
fft_inplace(spectrum, true);
|
||||
|
||||
result.bandwidth_hz = frequency_hz.back() - frequency_hz.front();
|
||||
const double dt_s = 1.0 / (static_cast<double>(fft_len) * df_hz);
|
||||
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<double>(index) * dt_s;
|
||||
result.depth_m[index] = result.time_s[index] * velocity_mps * 0.5;
|
||||
result.amplitude[index] = std::abs(spectrum[index]);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto legacy_peak_depth_for_domain(const LegacyPeakRecord& peak, LegacyPeakDomain domain) -> double {
|
||||
return domain == LegacyPeakDomain::Corrected ? peak.z_corr : peak.z_app;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto legacy_peak_tau_for_domain(const LegacyPeakRecord& peak, LegacyPeakDomain domain) -> double {
|
||||
return domain == LegacyPeakDomain::Corrected ? peak.tau_corr : peak.tau;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_legacy_depth_excluded(
|
||||
double z_value,
|
||||
const std::vector<std::pair<double, double>>& ranges
|
||||
) -> bool {
|
||||
for (const auto& [low, high] : ranges) {
|
||||
if (z_value >= low && z_value <= high) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_legacy_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, LegacyPairTiming> {
|
||||
std::unordered_map<PairKey, LegacyPairTiming> timing_by_pair{};
|
||||
timing_by_pair.reserve(traces.size());
|
||||
|
||||
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{});
|
||||
}
|
||||
return timing_by_pair;
|
||||
}
|
||||
|
||||
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"
|
||||
);
|
||||
}
|
||||
|
||||
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 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_by_pair;
|
||||
}
|
||||
|
||||
void apply_legacy_motion_correction(
|
||||
std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::unordered_map<PairKey, LegacyPairTiming>& 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 legacy 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_legacy_accumulator(
|
||||
const GridDefinition& grid,
|
||||
const std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::vector<std::pair<double, double>>& exclude_ranges,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
LegacyPeakDomain domain
|
||||
) -> std::vector<double> {
|
||||
const std::size_t width = grid.x_grid.size();
|
||||
const std::size_t height = grid.z_grid.size();
|
||||
std::vector<double> accumulator(width * height, 0.0);
|
||||
if (!(shell_sigma_m > 0.0)) {
|
||||
return accumulator;
|
||||
}
|
||||
|
||||
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<std::uint32_t>(tx_index), static_cast<std::uint32_t>(rx_index))
|
||||
);
|
||||
if (peak_it == peaks_by_pair.end()) {
|
||||
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_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const double range_total = velocity_mps * legacy_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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return accumulator;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto count_legacy_agreeing_ellipses(
|
||||
double x_est,
|
||||
double z_est,
|
||||
const std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::vector<std::pair<double, double>>& exclude_ranges,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m,
|
||||
LegacyPeakDomain 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<std::uint32_t>(tx_index), static_cast<std::uint32_t>(rx_index))
|
||||
);
|
||||
if (peak_it == peaks_by_pair.end()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (const auto& peak : peak_it->second) {
|
||||
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_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 * 6.0) {
|
||||
count += 1U;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return static_cast<double>(count);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto find_legacy_centroid(
|
||||
const std::vector<double>& values,
|
||||
std::size_t width,
|
||||
std::size_t height,
|
||||
std::size_t row,
|
||||
std::size_t col,
|
||||
std::size_t radius_z,
|
||||
std::size_t radius_x,
|
||||
const std::vector<double>& x_grid,
|
||||
const std::vector<double>& z_grid
|
||||
) -> std::pair<double, double> {
|
||||
if (values.empty()) {
|
||||
return {x_grid[col], z_grid[row]};
|
||||
}
|
||||
|
||||
const auto row_start = row > radius_z ? row - radius_z : 0U;
|
||||
const auto row_stop = std::min(height, row + radius_z + 1U);
|
||||
const auto col_start = col > radius_x ? col - radius_x : 0U;
|
||||
const auto col_stop = std::min(width, col + radius_x + 1U);
|
||||
|
||||
double weight_sum = 0.0;
|
||||
double row_weighted_sum = 0.0;
|
||||
double col_weighted_sum = 0.0;
|
||||
for (std::size_t sample_row = row_start; sample_row < row_stop; ++sample_row) {
|
||||
for (std::size_t sample_col = col_start; sample_col < col_stop; ++sample_col) {
|
||||
const double weight = values[(sample_row * width) + sample_col];
|
||||
weight_sum += weight;
|
||||
row_weighted_sum += static_cast<double>(sample_row) * weight;
|
||||
col_weighted_sum += static_cast<double>(sample_col) * weight;
|
||||
}
|
||||
}
|
||||
|
||||
if (!(weight_sum > 0.0)) {
|
||||
return {x_grid[col], z_grid[row]};
|
||||
}
|
||||
|
||||
const auto centroid_row =
|
||||
clamp_index(static_cast<std::ptrdiff_t>(std::llround(row_weighted_sum / weight_sum)), height);
|
||||
const auto centroid_col =
|
||||
clamp_index(static_cast<std::ptrdiff_t>(std::llround(col_weighted_sum / weight_sum)), width);
|
||||
return {x_grid[centroid_col], z_grid[centroid_row]};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto clean_legacy_find_points(
|
||||
const GridDefinition& grid,
|
||||
const std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_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 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)};
|
||||
}
|
||||
|
||||
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))));
|
||||
const auto radius_z =
|
||||
static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
|
||||
|
||||
std::vector<std::pair<double, double>> excluded_ranges{};
|
||||
for (std::size_t step = 0U; step < kLegacyMaxObjects; ++step) {
|
||||
const auto current = build_legacy_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(), kLegacySmoothSigma);
|
||||
const double smoothed_max = max_value(smoothed);
|
||||
if (!(smoothed_max > (kLegacyCleanThresholdFrac * initial_max))) {
|
||||
break;
|
||||
}
|
||||
|
||||
const auto max_it = std::max_element(smoothed.begin(), smoothed.end());
|
||||
const auto max_index = static_cast<std::size_t>(std::distance(smoothed.begin(), max_it));
|
||||
const auto peak_row = max_index / grid.x_grid.size();
|
||||
const auto peak_col = max_index % grid.x_grid.size();
|
||||
const auto [x_est, z_est] = find_legacy_centroid(
|
||||
smoothed,
|
||||
grid.x_grid.size(),
|
||||
grid.z_grid.size(),
|
||||
peak_row,
|
||||
peak_col,
|
||||
radius_z,
|
||||
radius_x,
|
||||
grid.x_grid,
|
||||
grid.z_grid
|
||||
);
|
||||
|
||||
found.push_back(
|
||||
LegacyPointRecord{
|
||||
.x_m = x_est,
|
||||
.z_m = z_est,
|
||||
.score = count_legacy_agreeing_ellipses(
|
||||
x_est,
|
||||
z_est,
|
||||
peaks_by_pair,
|
||||
excluded_ranges,
|
||||
x_tx,
|
||||
x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
domain
|
||||
),
|
||||
}
|
||||
);
|
||||
|
||||
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) {
|
||||
const auto peak_it = peaks_by_pair.find(
|
||||
make_pair_key(static_cast<std::uint32_t>(tx_index), static_cast<std::uint32_t>(rx_index))
|
||||
);
|
||||
if (peak_it == peaks_by_pair.end()) {
|
||||
continue;
|
||||
}
|
||||
for (const auto& peak : peak_it->second) {
|
||||
if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), 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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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(), kLegacySmoothSigma)};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto extended_legacy_find_regions(
|
||||
const GridDefinition& grid,
|
||||
const std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
|
||||
const std::vector<double>& x_tx,
|
||||
const std::vector<double>& x_rx,
|
||||
double velocity_mps,
|
||||
double shell_sigma_m
|
||||
) -> std::pair<std::vector<LegacyRegionRecord>, std::vector<double>> {
|
||||
std::vector<LegacyRegionRecord> regions{};
|
||||
const auto accumulator = build_legacy_accumulator(
|
||||
grid,
|
||||
peaks_by_pair,
|
||||
{},
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
x_tx,
|
||||
x_rx,
|
||||
LegacyPeakDomain::Apparent
|
||||
);
|
||||
const auto smoothed = gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kLegacySmoothSigma);
|
||||
const double smoothed_max = max_value(smoothed);
|
||||
if (!(smoothed_max > 0.0) || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) {
|
||||
return {regions, smoothed};
|
||||
}
|
||||
|
||||
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 = std::max(dx_cm * dz_cm, 1e-6);
|
||||
const auto min_pixels =
|
||||
static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyExtendedMinAreaCm2 / pixel_area_cm2)));
|
||||
|
||||
const std::size_t width = grid.x_grid.size();
|
||||
const std::size_t height = grid.z_grid.size();
|
||||
const double threshold = kLegacyExtendedThresholdFrac * smoothed_max;
|
||||
std::vector<std::uint8_t> 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<std::size_t> stack{start_index};
|
||||
std::vector<std::size_t> 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<std::ptrdiff_t, std::ptrdiff_t> offsets[] = {
|
||||
{-1, 0},
|
||||
{1, 0},
|
||||
{0, -1},
|
||||
{0, 1},
|
||||
};
|
||||
|
||||
for (const auto& [row_offset, col_offset] : offsets) {
|
||||
const auto next_row = static_cast<std::ptrdiff_t>(cell_row) + row_offset;
|
||||
const auto next_col = static_cast<std::ptrdiff_t>(cell_col) + col_offset;
|
||||
if (next_row < 0 || next_col < 0) {
|
||||
continue;
|
||||
}
|
||||
const bool row_out_of_bounds = next_row >= static_cast<std::ptrdiff_t>(height);
|
||||
const bool col_out_of_bounds = next_col >= static_cast<std::ptrdiff_t>(width);
|
||||
if (row_out_of_bounds || col_out_of_bounds) {
|
||||
continue;
|
||||
}
|
||||
const auto next_index =
|
||||
(static_cast<std::size_t>(next_row) * width) + static_cast<std::size_t>(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;
|
||||
}
|
||||
|
||||
LegacyRegionRecord 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_legacy_agreeing_ellipses(
|
||||
region.x_m,
|
||||
region.z_m,
|
||||
peaks_by_pair,
|
||||
{},
|
||||
x_tx,
|
||||
x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
LegacyPeakDomain::Apparent
|
||||
);
|
||||
region.pixel_count = static_cast<double>(component.size());
|
||||
regions.push_back(std::move(region));
|
||||
}
|
||||
}
|
||||
|
||||
return {regions, smoothed};
|
||||
}
|
||||
|
||||
[[nodiscard]] auto process_legacy_gpr(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = collection.collection_id;
|
||||
results.monotonic_ns = collection.monotonic_ns;
|
||||
|
||||
const auto selection = build_geometry_selection(run_config, live_config);
|
||||
if (selection.input_positions.empty() || selection.output_positions.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
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);
|
||||
if (selected_traces.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
const double velocity_mps =
|
||||
kSpeedOfLightMetersPerSec / std::sqrt(std::max(1e-6, static_cast<double>(run_config.gpr.relative_permittivity)));
|
||||
const double start_hz = static_cast<double>(live_config.gpr_start_freq_mhz) * 1'000'000.0;
|
||||
const double stop_hz = static_cast<double>(live_config.gpr_stop_freq_mhz) * 1'000'000.0;
|
||||
const double min_depth_m = static_cast<double>(live_config.gpr_min_depth_m);
|
||||
const double max_depth_m = static_cast<double>(live_config.gpr_max_depth_m);
|
||||
if (!(max_depth_m > min_depth_m)) {
|
||||
return results;
|
||||
}
|
||||
|
||||
std::unordered_map<PairKey, LegacyAscanResult> ascans_by_pair{};
|
||||
double bandwidth_hz = 0.0;
|
||||
for (const auto& trace : selected_traces) {
|
||||
auto ascan = compute_legacy_ascan(trace, start_hz, stop_hz, velocity_mps);
|
||||
if (ascan.amplitude.empty() || !(ascan.bandwidth_hz > 0.0)) {
|
||||
continue;
|
||||
}
|
||||
bandwidth_hz = std::max(bandwidth_hz, ascan.bandwidth_hz);
|
||||
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)) {
|
||||
return results;
|
||||
}
|
||||
|
||||
const auto grid = build_grid(selection.x_tx, selection.x_rx, max_depth_m, kLegacyGridZMinM);
|
||||
if (grid.x_grid.empty() || grid.z_grid.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
const double shell_sigma_m = velocity_mps / bandwidth_hz * 0.5;
|
||||
const double snr_thresh = std::max(0.0, static_cast<double>(live_config.gpr_snr_thresh));
|
||||
const double snr_comp_max = std::max(0.0, static_cast<double>(live_config.gpr_snr_comp_max));
|
||||
const double comp_power = std::max(0.0, static_cast<double>(live_config.gpr_comp_power));
|
||||
|
||||
std::unordered_map<PairKey, std::vector<LegacyPeakRecord>> 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<std::uint32_t>(tx_index), static_cast<std::uint32_t>(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, min_depth_m);
|
||||
const auto max_index = lower_bound_index(ascan.depth_m, max_depth_m);
|
||||
if (max_index <= min_index + 2U || max_index > ascan.amplitude.size()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto noise_begin = ascan.amplitude.begin() + static_cast<std::ptrdiff_t>(min_index);
|
||||
const auto noise_end = ascan.amplitude.begin() + static_cast<std::ptrdiff_t>(max_index);
|
||||
const double noise = median_copy(std::vector<double>(noise_begin, noise_end));
|
||||
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,
|
||||
std::floor(((velocity_mps / (2.0 * bandwidth_hz)) / z_step) * 0.7)
|
||||
));
|
||||
const auto peak_indices =
|
||||
find_legacy_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 =
|
||||
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);
|
||||
const double snr_comp = std::min(
|
||||
snr_raw / (std::pow(attenuation / attenuation_ref, comp_power) + 1e-12),
|
||||
snr_comp_max
|
||||
);
|
||||
peaks.push_back(
|
||||
LegacyPeakRecord{
|
||||
.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 (live_config.gpr_algorithm == GprAlgorithm::LegacyExtended) {
|
||||
const auto [regions, smoothed_accumulator] = extended_legacy_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<std::vector<float>> 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<float>(region.x_m),
|
||||
static_cast<float>(region.z_m),
|
||||
static_cast<float>(region.score),
|
||||
static_cast<float>(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<double>(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_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);
|
||||
|
||||
const auto [points, smoothed_accumulator] = clean_legacy_find_points(
|
||||
grid,
|
||||
peaks_by_pair,
|
||||
selection.x_tx,
|
||||
selection.x_rx,
|
||||
velocity_mps,
|
||||
shell_sigma_m,
|
||||
LegacyPeakDomain::Corrected
|
||||
);
|
||||
results.collection_payloads.push_back(
|
||||
build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator)
|
||||
);
|
||||
|
||||
std::vector<std::vector<float>> point_rows{};
|
||||
point_rows.reserve(points.size());
|
||||
for (const auto& point : points) {
|
||||
point_rows.push_back(
|
||||
{
|
||||
static_cast<float>(point.x_m),
|
||||
static_cast<float>(point.z_m),
|
||||
static_cast<float>(point.score),
|
||||
}
|
||||
);
|
||||
}
|
||||
results.collection_payloads.push_back(build_table_payload("gpr_points", point_rows, 3U));
|
||||
return results;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user