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

new GPR

Browse Source
This commit is contained in:
Ayzen
2026-06-11 19:51:30 +03:00
parent 9661504e51
commit f0d095de80
16 changed files with 1786 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp
+283 -53
View File
@@ -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
+1
View File
@@ -10,6 +10,7 @@
#include <queue>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <unordered_set>