@@ -74,28 +74,7 @@ struct ProcessingLiveConfig {
|
||||
bool gpr_remove_sidelobe_objects_enabled = true;
|
||||
// BP image is computed in the y=imaging_plane_y_m slice of the 3D grid.
|
||||
// Default 0 keeps legacy 1D antenna layouts imaging in the antenna plane.
|
||||
// Used only when gpr_plane_mode == "const".
|
||||
float gpr_imaging_plane_y_m = 0.0F;
|
||||
// Acquisition timing model for motion correction (notebook
|
||||
// FREQUENCY_TIMING_MODE): "segmented_interleaved" — Tx outputs run in
|
||||
// sequential segments, each segment sweeps all frequencies over its window
|
||||
// while its Tx interleave per frequency; "interleaved" — legacy model where
|
||||
// all Tx interleave over the whole frame.
|
||||
std::string gpr_timing_mode = "segmented_interleaved";
|
||||
// Fallback segment size when per-trace capture timestamps are absent;
|
||||
// timestamp grouping wins when available.
|
||||
std::uint32_t gpr_tx_per_segment = 2U;
|
||||
// Imaging plane of the 2D BP map (notebook BP_PLANE_MODE): "ground" — the
|
||||
// plane follows the ground given the frame pitch, needed once the antennas
|
||||
// are spread vertically; "const" — legacy fixed y = gpr_imaging_plane_y_m.
|
||||
std::string gpr_plane_mode = "ground";
|
||||
// Height of the LOWER antenna row above the ground, metres (notebook
|
||||
// BP_GROUND_DEPTH_M; structural constant of the frame).
|
||||
float gpr_ground_depth_m = 1.25F;
|
||||
// Frame pitch used by the ground plane. Override disabled -> the pitch
|
||||
// falls back to gpr_look_angle_deg (notebook BP_GROUND_PITCH_DEG = None).
|
||||
bool gpr_ground_pitch_override_enabled = false;
|
||||
float gpr_ground_pitch_deg = 6.5F;
|
||||
// Coherent BP object visibility (window + the draw limits below) is applied in
|
||||
// the processor itself, matching Horns_motion_3libre.py — there is NO score
|
||||
// threshold for it. Only legacy GPR still thresholds, on a pair count.
|
||||
|
||||
@@ -67,20 +67,6 @@ using Json = nlohmann::json;
|
||||
throw std::runtime_error(field_name + " must be one of: int_minus, int_focus");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_timing_mode(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "segmented_interleaved" || value == "interleaved") {
|
||||
return value;
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: segmented_interleaved, interleaved");
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_gpr_plane_mode(const std::string& value, const std::string& field_name) -> std::string {
|
||||
if (value == "ground" || value == "const") {
|
||||
return value;
|
||||
}
|
||||
throw std::runtime_error(field_name + " must be one of: ground, const");
|
||||
}
|
||||
|
||||
void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) {
|
||||
if (value == "backprojection") {
|
||||
return;
|
||||
@@ -369,45 +355,6 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
}
|
||||
config.gpr_imaging_plane_y_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_timing_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_timing_mode must be string");
|
||||
}
|
||||
config.gpr_timing_mode = parse_gpr_timing_mode(found->get<std::string>(), "processing.gpr_timing_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_tx_per_segment"); found != root.end()) {
|
||||
config.gpr_tx_per_segment = parse_u32_number(*found, "processing.gpr_tx_per_segment");
|
||||
if (config.gpr_tx_per_segment == 0U) {
|
||||
throw std::runtime_error("processing.gpr_tx_per_segment must be >= 1");
|
||||
}
|
||||
}
|
||||
if (const auto found = root.find("gpr_plane_mode"); found != root.end()) {
|
||||
if (!found->is_string()) {
|
||||
throw std::runtime_error("processing.gpr_plane_mode must be string");
|
||||
}
|
||||
config.gpr_plane_mode = parse_gpr_plane_mode(found->get<std::string>(), "processing.gpr_plane_mode");
|
||||
}
|
||||
if (const auto found = root.find("gpr_ground_depth_m"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_ground_depth_m must be number");
|
||||
}
|
||||
config.gpr_ground_depth_m = static_cast<float>(found->get<double>());
|
||||
if (config.gpr_ground_depth_m < 0.0F) {
|
||||
throw std::runtime_error("processing.gpr_ground_depth_m must be >= 0");
|
||||
}
|
||||
}
|
||||
if (const auto found = root.find("gpr_ground_pitch_override_enabled"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.gpr_ground_pitch_override_enabled must be bool");
|
||||
}
|
||||
config.gpr_ground_pitch_override_enabled = found->get<bool>();
|
||||
}
|
||||
if (const auto found = root.find("gpr_ground_pitch_deg"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_ground_pitch_deg must be number");
|
||||
}
|
||||
config.gpr_ground_pitch_deg = static_cast<float>(found->get<double>());
|
||||
}
|
||||
for (const auto& [key, target] : {
|
||||
std::pair{"gpr_visible_x_min_m", &config.gpr_visible_x_min_m},
|
||||
std::pair{"gpr_visible_x_max_m", &config.gpr_visible_x_max_m},
|
||||
|
||||
+63
-304
@@ -653,242 +653,52 @@ void validate_collection_trace_order(
|
||||
return traces;
|
||||
}
|
||||
|
||||
// ── Frame acquisition timing ────────────────────────────────────────────────
|
||||
//
|
||||
// The real acquisition is SEGMENTED (notebook FREQUENCY_TIMING_MODE =
|
||||
// 'segmented_interleaved'): the Tx outputs are grouped into segments that run
|
||||
// one after another; each segment sweeps ALL frequencies over its own window,
|
||||
// and within a segment its Tx interleave per frequency while all Rx receive in
|
||||
// parallel. The dominant motion shift is therefore BETWEEN segments (a pair-
|
||||
// level dtau relative to the frame centre), with a much smaller intra-segment
|
||||
// sweep shift on top — the legacy whole-frame interleaved model captured only
|
||||
// the latter.
|
||||
//
|
||||
// Segment windows come from the per-trace capture timestamps: traces measured
|
||||
// in one segment share one (start, end) window on both producers, so grouping
|
||||
// outputs by identical windows reconstructs the segments exactly (notebook
|
||||
// snapshot_acquisition_timing). When timestamps are absent (zero sentinel) the
|
||||
// frame is split uniformly into ceil(n_tx / tx_per_segment) segments.
|
||||
//
|
||||
// Segment/slot assignment is a hardware property: it is derived from the FULL
|
||||
// run_combos order, never from the live Tx selection — otherwise disabling
|
||||
// channels would silently shrink the frame model (this also fixes the old
|
||||
// capture_span / N_selected_tx bug).
|
||||
|
||||
struct SegmentWindow {
|
||||
double start_s = 0.0;
|
||||
double end_s = 0.0;
|
||||
double center_s = 0.0;
|
||||
bool stretched = false;
|
||||
};
|
||||
|
||||
struct TxSlot {
|
||||
std::size_t segment_index = 0U;
|
||||
std::size_t slot = 0U;
|
||||
std::size_t slot_count = 1U;
|
||||
};
|
||||
|
||||
struct FrameTiming {
|
||||
std::vector<SegmentWindow> segments{};
|
||||
std::unordered_map<std::uint32_t, TxSlot> by_output_pos{};
|
||||
double frame_center_s = 0.0;
|
||||
bool from_trace_timestamps = false;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto build_frame_timing(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::size_t tx_per_segment_fallback,
|
||||
bool segmented
|
||||
) -> FrameTiming {
|
||||
FrameTiming timing{};
|
||||
|
||||
// Full hardware Tx order: first occurrence of each output in run_combos
|
||||
// (notebook tx_order_full). Deliberately ignores the live Tx selection.
|
||||
std::vector<std::uint32_t> tx_order{};
|
||||
for (const auto& combo : run_config.run_combos) {
|
||||
if (std::find(tx_order.begin(), tx_order.end(), combo.output_pos) == tx_order.end()) {
|
||||
tx_order.push_back(combo.output_pos);
|
||||
}
|
||||
}
|
||||
if (tx_order.empty()) {
|
||||
return timing;
|
||||
}
|
||||
|
||||
const double frame_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;
|
||||
|
||||
// Timestamp path: per output take the window of its FIRST trace (notebook
|
||||
// setdefault); require every window measured. Traces of one segment share
|
||||
// one window by construction, so identical windows identify segments.
|
||||
std::unordered_map<std::uint32_t, std::pair<std::uint64_t, std::uint64_t>> window_by_tx{};
|
||||
bool timestamps_ok = segmented;
|
||||
if (segmented) {
|
||||
for (const auto& trace : collection.traces) {
|
||||
window_by_tx.try_emplace(
|
||||
trace.combo.output_pos,
|
||||
std::pair{trace.capture_start_ns, trace.capture_end_ns}
|
||||
);
|
||||
}
|
||||
for (const auto tx : tx_order) {
|
||||
const auto found = window_by_tx.find(tx);
|
||||
if (found == window_by_tx.end() || found->second.second <= found->second.first) {
|
||||
timestamps_ok = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (timestamps_ok) {
|
||||
std::uint64_t first_start_ns = std::numeric_limits<std::uint64_t>::max();
|
||||
std::uint64_t last_end_ns = 0U;
|
||||
for (const auto tx : tx_order) {
|
||||
const auto& window = window_by_tx.at(tx);
|
||||
first_start_ns = std::min(first_start_ns, window.first);
|
||||
last_end_ns = std::max(last_end_ns, window.second);
|
||||
}
|
||||
const double acquisition_s = static_cast<double>(last_end_ns - first_start_ns) * 1e-9;
|
||||
|
||||
// Unique windows ordered by start; Tx sharing a window form one segment,
|
||||
// slotted in hardware order.
|
||||
std::vector<std::pair<std::uint64_t, std::uint64_t>> windows{};
|
||||
for (const auto tx : tx_order) {
|
||||
const auto& window = window_by_tx.at(tx);
|
||||
if (std::find(windows.begin(), windows.end(), window) == windows.end()) {
|
||||
windows.push_back(window);
|
||||
}
|
||||
}
|
||||
std::sort(windows.begin(), windows.end());
|
||||
|
||||
const double nominal_segment_s =
|
||||
acquisition_s / static_cast<double>(std::max<std::size_t>(windows.size(), 1U));
|
||||
timing.segments.reserve(windows.size());
|
||||
for (const auto& window : windows) {
|
||||
SegmentWindow segment{};
|
||||
segment.start_s = static_cast<double>(window.first - first_start_ns) * 1e-9;
|
||||
segment.end_s = static_cast<double>(window.second - first_start_ns) * 1e-9;
|
||||
segment.center_s = 0.5 * (segment.start_s + segment.end_s);
|
||||
// A segment noticeably longer than the frame/n_segments norm had an
|
||||
// instrument stall inside; where exactly is unknowable from the
|
||||
// timestamps, so the uniform frequency layout is approximate there.
|
||||
segment.stretched =
|
||||
nominal_segment_s > 0.0 && (segment.end_s - segment.start_s) > 1.03 * nominal_segment_s;
|
||||
timing.segments.push_back(segment);
|
||||
}
|
||||
|
||||
for (const auto tx : tx_order) {
|
||||
const auto& window = window_by_tx.at(tx);
|
||||
const auto segment_index = static_cast<std::size_t>(
|
||||
std::find(windows.begin(), windows.end(), window) - windows.begin()
|
||||
);
|
||||
TxSlot slot{};
|
||||
slot.segment_index = segment_index;
|
||||
slot.slot = 0U;
|
||||
for (const auto other : tx_order) {
|
||||
if (other == tx) {
|
||||
break;
|
||||
}
|
||||
if (window_by_tx.at(other) == window) {
|
||||
++slot.slot;
|
||||
}
|
||||
}
|
||||
slot.slot_count = static_cast<std::size_t>(std::count_if(
|
||||
tx_order.begin(), tx_order.end(),
|
||||
[&](std::uint32_t other) { return window_by_tx.at(other) == window; }
|
||||
));
|
||||
timing.by_output_pos.emplace(tx, slot);
|
||||
}
|
||||
|
||||
timing.frame_center_s = 0.5 * acquisition_s;
|
||||
timing.from_trace_timestamps = true;
|
||||
return timing;
|
||||
}
|
||||
|
||||
// Fallback: uniform split of the collection-level span. `segmented` divides
|
||||
// the outputs into chunks of tx_per_segment; the legacy 'interleaved' mode is
|
||||
// the same code with a single segment holding every Tx.
|
||||
const std::size_t per_segment = segmented ? std::max<std::size_t>(tx_per_segment_fallback, 1U)
|
||||
: tx_order.size();
|
||||
const std::size_t segment_count = (tx_order.size() + per_segment - 1U) / per_segment;
|
||||
const double segment_span_s = frame_span_s / static_cast<double>(segment_count);
|
||||
timing.segments.reserve(segment_count);
|
||||
for (std::size_t index = 0U; index < segment_count; ++index) {
|
||||
SegmentWindow segment{};
|
||||
segment.start_s = static_cast<double>(index) * segment_span_s;
|
||||
segment.end_s = segment.start_s + segment_span_s;
|
||||
segment.center_s = 0.5 * (segment.start_s + segment.end_s);
|
||||
timing.segments.push_back(segment);
|
||||
}
|
||||
for (std::size_t index = 0U; index < tx_order.size(); ++index) {
|
||||
TxSlot slot{};
|
||||
slot.segment_index = index / per_segment;
|
||||
slot.slot = index % per_segment;
|
||||
const std::size_t segment_begin = slot.segment_index * per_segment;
|
||||
slot.slot_count = std::min(per_segment, tx_order.size() - segment_begin);
|
||||
timing.by_output_pos.emplace(tx_order[index], slot);
|
||||
}
|
||||
timing.frame_center_s = 0.5 * frame_span_s;
|
||||
return timing;
|
||||
}
|
||||
|
||||
// Frequency-domain speed correction applied to S21 *before* the IFFT, ported
|
||||
// from the Multi_antenna_3D notebook (apply_intra_sweep_phase_correction +
|
||||
// compute_frequency_sample_times, segmented branch). One phase model covers
|
||||
// both motion terms:
|
||||
// dtau_pair — the pair-level shift of this Tx's segment centre against the
|
||||
// frame centre (dominant between sequential segments);
|
||||
// dtau_intra — the within-segment sweep shift: frequency k of interleave
|
||||
// slot `slot` is event (slot_count*k + slot) of the segment.
|
||||
// The combined phi = 2*pi*f*(dtau_intra + dtau_pair) is applied as
|
||||
// s21 *= exp(-i*phi), matching the notebook's motion_phase_sign = -1.
|
||||
// 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.
|
||||
//
|
||||
// Every physical input is taken from the live system: `speed_mps` is the
|
||||
// socket-fed velocity, `look_angle_deg`/`direction_sign` are config, and the
|
||||
// segment windows come from the collection's own trace timestamps. A zero
|
||||
// speed or an empty segment window makes this a no-op.
|
||||
void apply_motion_correction(
|
||||
// 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,
|
||||
const SegmentWindow& segment,
|
||||
std::size_t slot,
|
||||
std::size_t slot_count,
|
||||
double frame_center_s,
|
||||
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();
|
||||
const double segment_span_s = segment.end_s - segment.start_s;
|
||||
if (n_full < 2U || trace.s21.size() != n_full || slot_count == 0U
|
||||
|| !(velocity_mps > 0.0) || !(segment_span_s > 0.0)) {
|
||||
|| !(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);
|
||||
|
||||
// The segment sweeps all frequencies over its window; on each frequency its
|
||||
// Tx take turns, so frequency k of slot `slot` is event (slot_count*k + slot)
|
||||
// out of slot_count*n_full.
|
||||
const double dt_event_s = segment_span_s / (static_cast<double>(slot_count) * static_cast<double>(n_full));
|
||||
// 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 segment.start_s
|
||||
+ ((static_cast<double>(slot_count) * static_cast<double>(index)) + static_cast<double>(slot))
|
||||
* dt_event_s;
|
||||
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);
|
||||
|
||||
// Pair-level coarse shift: segment centre vs frame centre.
|
||||
const double dz_pair_m = motion_factor * (segment.center_s - frame_center_s);
|
||||
const double dtau_pair_s = 2.0 * dz_pair_m / velocity_mps;
|
||||
|
||||
std::vector<std::size_t> band_indices{};
|
||||
std::vector<double> band_frequency_hz{};
|
||||
std::vector<double> phi{};
|
||||
@@ -902,9 +712,10 @@ void apply_motion_correction(
|
||||
if (frequency < low_hz || frequency > high_hz) {
|
||||
continue;
|
||||
}
|
||||
const double dt_intra_s = sample_time_s(index) - segment.center_s;
|
||||
const double delta_path_m = 2.0 * motion_factor * dt_intra_s;
|
||||
const double dtau_s = (delta_path_m / velocity_mps) + dtau_pair_s;
|
||||
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;
|
||||
}
|
||||
@@ -1081,32 +892,11 @@ void normalize_pair_ascans(
|
||||
return std::sqrt((dx * dx) + (dy * dy) + (dz * dz));
|
||||
}
|
||||
|
||||
// Y of the BP imaging plane at range z. In 'ground' mode the plane follows the
|
||||
// ground in the radar's tilted axes (notebook bp_plane_y): the frame is pitched
|
||||
// down by pitch_deg and the lower antenna row sits ground_depth_m above ground,
|
||||
// so y(z) = (-ground_depth_m + z*sin(pitch)) / cos(pitch). With antennas spread
|
||||
// vertically an off-plane point smears into an equal-range arc, so the plane
|
||||
// choice matters; 'const' keeps the legacy fixed-y slice.
|
||||
struct ImagingPlane {
|
||||
bool ground_mode = false;
|
||||
double const_y_m = 0.0;
|
||||
double ground_depth_m = 0.0;
|
||||
double pitch_deg = 0.0;
|
||||
|
||||
[[nodiscard]] auto y_at(double z_m) const -> double {
|
||||
if (!ground_mode) {
|
||||
return const_y_m;
|
||||
}
|
||||
const double pitch_rad = (pitch_deg * kPi) / 180.0;
|
||||
return (-ground_depth_m + (z_m * std::sin(pitch_rad))) / std::cos(pitch_rad);
|
||||
}
|
||||
};
|
||||
|
||||
[[nodiscard]] auto build_grid(
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
const ImagingPlane& plane
|
||||
double imaging_plane_y_m
|
||||
) -> GridDefinition {
|
||||
GridDefinition grid{};
|
||||
if (selection.x_tx.empty() || selection.x_rx.empty() || !(max_depth_m > min_z_m)) {
|
||||
@@ -1127,8 +917,6 @@ struct ImagingPlane {
|
||||
|
||||
for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) {
|
||||
const double z_value = grid.z_grid[row];
|
||||
// Plane height depends only on range, so it is constant along a row.
|
||||
const double plane_y_m = plane.y_at(z_value);
|
||||
for (std::size_t col = 0U; col < grid.x_grid.size(); ++col) {
|
||||
const double x_value = grid.x_grid[col];
|
||||
const auto cell_index = (row * grid.x_grid.size()) + col;
|
||||
@@ -1136,14 +924,14 @@ struct ImagingPlane {
|
||||
for (std::size_t tx_index = 0U; tx_index < selection.x_tx.size(); ++tx_index) {
|
||||
grid.tx_distance_grids[tx_index][cell_index] = distance_3d(
|
||||
x_value - selection.x_tx[tx_index],
|
||||
plane_y_m - selection.y_tx[tx_index],
|
||||
imaging_plane_y_m - selection.y_tx[tx_index],
|
||||
z_value - selection.z_tx[tx_index]
|
||||
);
|
||||
}
|
||||
for (std::size_t rx_index = 0U; rx_index < selection.x_rx.size(); ++rx_index) {
|
||||
grid.rx_distance_grids[rx_index][cell_index] = distance_3d(
|
||||
x_value - selection.x_rx[rx_index],
|
||||
plane_y_m - selection.y_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
z_value - selection.z_rx[rx_index]
|
||||
);
|
||||
}
|
||||
@@ -1160,10 +948,10 @@ struct ImagingPlane {
|
||||
const GeometrySelection& selection,
|
||||
double max_depth_m,
|
||||
double min_z_m,
|
||||
const ImagingPlane& plane
|
||||
double imaging_plane_y_m
|
||||
) -> std::vector<double> {
|
||||
std::vector<double> signature;
|
||||
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 6U);
|
||||
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 3U);
|
||||
const auto append = [&](const std::vector<double>& axis) {
|
||||
signature.insert(signature.end(), axis.begin(), axis.end());
|
||||
};
|
||||
@@ -1175,10 +963,7 @@ struct ImagingPlane {
|
||||
append(selection.z_rx);
|
||||
signature.push_back(max_depth_m);
|
||||
signature.push_back(min_z_m);
|
||||
signature.push_back(plane.ground_mode ? 1.0 : 0.0);
|
||||
signature.push_back(plane.const_y_m);
|
||||
signature.push_back(plane.ground_depth_m);
|
||||
signature.push_back(plane.pitch_deg);
|
||||
signature.push_back(imaging_plane_y_m);
|
||||
return signature;
|
||||
}
|
||||
|
||||
@@ -1218,20 +1003,19 @@ struct ImagingPlane {
|
||||
std::uint32_t tx_index,
|
||||
std::uint32_t rx_index,
|
||||
const GeometrySelection& selection,
|
||||
const ImagingPlane& plane
|
||||
double imaging_plane_y_m
|
||||
) -> std::pair<double, double> {
|
||||
const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
|
||||
const double plane_y_m = plane.y_at(kCompensationReferenceDepthM);
|
||||
const double dz_tx = kCompensationReferenceDepthM - selection.z_tx[tx_index];
|
||||
const double dz_rx = kCompensationReferenceDepthM - selection.z_rx[rx_index];
|
||||
const double r_tx = distance_3d(
|
||||
x_center - selection.x_tx[tx_index],
|
||||
plane_y_m - selection.y_tx[tx_index],
|
||||
imaging_plane_y_m - selection.y_tx[tx_index],
|
||||
dz_tx
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
x_center - selection.x_rx[rx_index],
|
||||
plane_y_m - selection.y_rx[rx_index],
|
||||
imaging_plane_y_m - selection.y_rx[rx_index],
|
||||
dz_rx
|
||||
);
|
||||
return attenuation_components(r_tx, r_rx, dz_tx, dz_rx);
|
||||
@@ -1269,7 +1053,7 @@ struct ImagingPlane {
|
||||
const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
|
||||
const GridDefinition& grid,
|
||||
const GeometrySelection& selection,
|
||||
const ImagingPlane& plane,
|
||||
double imaging_plane_y_m,
|
||||
double velocity_mps,
|
||||
double min_depth_m,
|
||||
double max_depth_m,
|
||||
@@ -1311,7 +1095,7 @@ struct ImagingPlane {
|
||||
trace.tx_local_index,
|
||||
trace.rx_local_index,
|
||||
selection,
|
||||
plane
|
||||
imaging_plane_y_m
|
||||
);
|
||||
contributions.push_back(PairContribution{
|
||||
&grid.tx_distance_grids[trace.tx_local_index],
|
||||
@@ -1721,22 +1505,21 @@ void apply_depth_gate(
|
||||
const ObjectRecord& object,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
const ImagingPlane& plane
|
||||
double imaging_plane_y_m
|
||||
) -> std::vector<double> {
|
||||
std::vector<double> signature{};
|
||||
signature.reserve(selected_traces.size());
|
||||
const double plane_y_m = plane.y_at(object.z_m);
|
||||
for (const auto& trace : selected_traces) {
|
||||
const auto tx = trace.tx_local_index;
|
||||
const auto rx = trace.rx_local_index;
|
||||
const double r_tx = distance_3d(
|
||||
object.x_m - selection.x_tx[tx],
|
||||
plane_y_m - selection.y_tx[tx],
|
||||
imaging_plane_y_m - selection.y_tx[tx],
|
||||
object.z_m - selection.z_tx[tx]
|
||||
);
|
||||
const double r_rx = distance_3d(
|
||||
object.x_m - selection.x_rx[rx],
|
||||
plane_y_m - selection.y_rx[rx],
|
||||
imaging_plane_y_m - selection.y_rx[rx],
|
||||
object.z_m - selection.z_rx[rx]
|
||||
);
|
||||
signature.push_back(0.5 * (r_tx + r_rx));
|
||||
@@ -1748,12 +1531,12 @@ void mark_sidelobe_candidates(
|
||||
std::vector<ObjectRecord>& objects,
|
||||
const std::vector<SelectedTrace>& selected_traces,
|
||||
const GeometrySelection& selection,
|
||||
const ImagingPlane& plane
|
||||
double imaging_plane_y_m
|
||||
) {
|
||||
std::vector<std::vector<double>> signatures{};
|
||||
signatures.reserve(objects.size());
|
||||
for (const auto& object : objects) {
|
||||
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, plane));
|
||||
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, imaging_plane_y_m));
|
||||
}
|
||||
|
||||
for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
|
||||
@@ -2088,48 +1871,34 @@ void add_bp_score_metrics(
|
||||
return results;
|
||||
}
|
||||
|
||||
// Motion correction before the IFFT: pair-level (between segments) plus
|
||||
// intra-segment sweep shift, one phase model (see apply_motion_correction).
|
||||
// The frame timing is built from the FULL collection — including traces of
|
||||
// deselected Tx — so disabling channels never shrinks the frame model.
|
||||
// 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 auto frame_timing = build_frame_timing(
|
||||
run_config,
|
||||
collection,
|
||||
static_cast<std::size_t>(std::max<std::uint32_t>(live_config.gpr_tx_per_segment, 1U)),
|
||||
live_config.gpr_timing_mode == "segmented_interleaved"
|
||||
);
|
||||
for (const auto& segment : frame_timing.segments) {
|
||||
if (segment.stretched) {
|
||||
std::cerr << "[gpr] collection " << collection.collection_id
|
||||
<< ": segment at " << segment.start_s << "s runs "
|
||||
<< (segment.end_s - segment.start_s)
|
||||
<< "s (above the frame/n_segments norm) — the instrument stalled "
|
||||
<< "inside it, motion correction for this frame is approximate\n";
|
||||
}
|
||||
}
|
||||
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) {
|
||||
const auto slot_it = frame_timing.by_output_pos.find(trace.combo.output_pos);
|
||||
if (slot_it == frame_timing.by_output_pos.end()) {
|
||||
continue;
|
||||
}
|
||||
const auto& tx_slot = slot_it->second;
|
||||
apply_motion_correction(
|
||||
apply_intra_sweep_motion_correction(
|
||||
trace,
|
||||
frame_timing.segments[tx_slot.segment_index],
|
||||
tx_slot.slot,
|
||||
tx_slot.slot_count,
|
||||
frame_timing.frame_center_s,
|
||||
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
|
||||
);
|
||||
}
|
||||
@@ -2150,28 +1919,18 @@ void add_bp_score_metrics(
|
||||
}
|
||||
normalize_pair_ascans(ascans_by_pair, velocity_mps, min_depth_m, max_depth_m);
|
||||
|
||||
// 'ground' plane pitch falls back to the look angle when the override is
|
||||
// off (notebook BP_GROUND_PITCH_DEG = None -> LOOK_ANGLE_DEG).
|
||||
const ImagingPlane plane{
|
||||
live_config.gpr_plane_mode == "ground",
|
||||
static_cast<double>(live_config.gpr_imaging_plane_y_m),
|
||||
static_cast<double>(live_config.gpr_ground_depth_m),
|
||||
live_config.gpr_ground_pitch_override_enabled
|
||||
? static_cast<double>(live_config.gpr_ground_pitch_deg)
|
||||
: static_cast<double>(live_config.gpr_look_angle_deg),
|
||||
};
|
||||
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
|
||||
|
||||
// The imaging grid (90k cells x 6 distance fields) only depends on antenna
|
||||
// geometry, the depth window and the imaging plane, which are constant
|
||||
// across a run, so it is memoized between collections and rebuilt only when
|
||||
// that signature changes. process_backprojection_gpr is entered by a single
|
||||
// processing thread (the backprojection workers join before it returns), so
|
||||
// a local static is safe.
|
||||
// geometry and the depth window, which are constant across a run, so it is
|
||||
// memoized between collections and rebuilt only when that signature changes.
|
||||
// process_backprojection_gpr is entered by a single processing thread (the
|
||||
// backprojection workers join before it returns), so a local static is safe.
|
||||
static std::vector<double> cached_grid_signature;
|
||||
static GridDefinition cached_grid;
|
||||
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, plane);
|
||||
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
if (cached_grid.x_grid.empty() || signature != cached_grid_signature) {
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, plane);
|
||||
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
|
||||
cached_grid_signature = signature;
|
||||
}
|
||||
const GridDefinition& grid = cached_grid;
|
||||
@@ -2184,7 +1943,7 @@ void add_bp_score_metrics(
|
||||
ascans_by_pair,
|
||||
grid,
|
||||
selection,
|
||||
plane,
|
||||
imaging_plane_y_m,
|
||||
velocity_mps,
|
||||
min_depth_m,
|
||||
max_depth_m,
|
||||
@@ -2202,7 +1961,7 @@ void add_bp_score_metrics(
|
||||
auto objects = find_bp_objects(display_map, grid, static_cast<double>(live_config.gpr_object_min_frac));
|
||||
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, plane);
|
||||
mark_sidelobe_candidates(objects, selected_traces, selection, imaging_plane_y_m);
|
||||
add_bp_score_metrics(objects, live_config);
|
||||
|
||||
if (live_config.gpr_remove_sidelobe_objects_enabled) {
|
||||
|
||||
@@ -918,12 +918,7 @@ void apply_legacy_motion_correction(
|
||||
return results;
|
||||
}
|
||||
|
||||
// Legacy GPR always images in the fixed-y slice; the ground plane mode is a
|
||||
// coherent-BP-only feature.
|
||||
const auto grid = build_grid(
|
||||
selection, max_depth_m, kLegacyGridZMinM,
|
||||
ImagingPlane{false, imaging_plane_y_m, 0.0, 0.0}
|
||||
);
|
||||
const auto grid = build_grid(selection, max_depth_m, kLegacyGridZMinM, imaging_plane_y_m);
|
||||
if (grid.x_grid.empty() || grid.z_grid.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
@@ -5,7 +5,6 @@
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <queue>
|
||||
|
||||
@@ -1,170 +0,0 @@
|
||||
// Numeric check of the GPR backprojection frame-timing model against the
|
||||
// Multi_antenna_3D notebook (compute_pair_timestamps_for_snapshot +
|
||||
// apply_intra_sweep_phase_correction, 'segmented_interleaved').
|
||||
//
|
||||
// Builds a synthetic 4x4 collection with two segment windows taken from
|
||||
// per-trace capture timestamps, runs build_frame_timing and
|
||||
// apply_motion_correction from gpr_backprojection_processor.ipp, and prints
|
||||
// segment/slot assignment, frame centre, per-Tx pair dtau and the motion phase
|
||||
// at probe frequency indices. Compare against the Python replica
|
||||
// (scripts/gpr_timing_check_reference.py) — every printed number must match.
|
||||
//
|
||||
// Also prints the fallback (zero timestamps) and legacy 'interleaved' layouts.
|
||||
//
|
||||
// Usage: gpr_timing_check
|
||||
#include "gpr_processor.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <queue>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <tuple>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
|
||||
#include "../processors/src/gpr_backprojection_processor.ipp"
|
||||
|
||||
constexpr double kSpeedMps = 1.42;
|
||||
constexpr double kLookAngleDeg = 19.5;
|
||||
constexpr double kDirectionSign = 1.0;
|
||||
constexpr std::size_t kFreqCount = 11U;
|
||||
|
||||
[[nodiscard]] auto make_run_config() -> config::RunConfig {
|
||||
config::RunConfig run_config{};
|
||||
for (std::uint32_t output = 0U; output < 4U; ++output) {
|
||||
for (std::uint32_t input = 0U; input < 4U; ++input) {
|
||||
run_config.run_combos.push_back(ipc::ComboKey{input, output});
|
||||
}
|
||||
}
|
||||
return run_config;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_collection(bool with_trace_timestamps) -> ipc::PreprocessedCollection {
|
||||
ipc::PreprocessedCollection collection{};
|
||||
collection.collection_id = 7U;
|
||||
// The collection-level capture starts 10 ms before the first trace — the
|
||||
// trace marks, not the collection window, must define the frame model.
|
||||
collection.capture_start_ns = 990'000'000ULL;
|
||||
collection.capture_end_ns = 1'483'000'000ULL;
|
||||
|
||||
// Segment A (Tx0, Tx1): 1.000..1.223 s; segment B (Tx2, Tx3): 1.223..1.483 s
|
||||
// (260 ms against the 241.5 ms norm -> stretched).
|
||||
const auto window_for_tx = [&](std::uint32_t output) -> std::pair<std::uint64_t, std::uint64_t> {
|
||||
if (!with_trace_timestamps) {
|
||||
return {0U, 0U};
|
||||
}
|
||||
return output < 2U
|
||||
? std::pair<std::uint64_t, std::uint64_t>{1'000'000'000ULL, 1'223'000'000ULL}
|
||||
: std::pair<std::uint64_t, std::uint64_t>{1'223'000'000ULL, 1'483'000'000ULL};
|
||||
};
|
||||
|
||||
for (std::uint32_t output = 0U; output < 4U; ++output) {
|
||||
for (std::uint32_t input = 0U; input < 4U; ++input) {
|
||||
ipc::SweepTraceBlock trace{};
|
||||
trace.combo = ipc::ComboKey{input, output};
|
||||
const auto window = window_for_tx(output);
|
||||
trace.capture_start_ns = window.first;
|
||||
trace.capture_end_ns = window.second;
|
||||
collection.traces.push_back(std::move(trace));
|
||||
}
|
||||
}
|
||||
return collection;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto make_trace(std::uint32_t output_pos) -> SelectedTrace {
|
||||
SelectedTrace trace{};
|
||||
trace.combo = ipc::ComboKey{0U, output_pos};
|
||||
trace.frequency_hz.resize(kFreqCount);
|
||||
trace.s21.assign(kFreqCount, std::complex<double>(1.0, 0.0));
|
||||
for (std::size_t index = 0U; index < kFreqCount; ++index) {
|
||||
trace.frequency_hz[index] =
|
||||
1.5e9 + (static_cast<double>(index) * (6.0e9 - 1.5e9) / static_cast<double>(kFreqCount - 1U));
|
||||
}
|
||||
return trace;
|
||||
}
|
||||
|
||||
void print_timing(const char* label, const FrameTiming& timing) {
|
||||
std::printf("%s: frame_center_s=%.9f from_timestamps=%d\n",
|
||||
label, timing.frame_center_s, timing.from_trace_timestamps ? 1 : 0);
|
||||
for (std::size_t index = 0U; index < timing.segments.size(); ++index) {
|
||||
const auto& segment = timing.segments[index];
|
||||
std::printf(" segment %zu: start=%.9f end=%.9f center=%.9f stretched=%d\n",
|
||||
index, segment.start_s, segment.end_s, segment.center_s,
|
||||
segment.stretched ? 1 : 0);
|
||||
}
|
||||
std::vector<std::uint32_t> outputs{};
|
||||
outputs.reserve(timing.by_output_pos.size());
|
||||
for (const auto& [output, _slot] : timing.by_output_pos) {
|
||||
outputs.push_back(output);
|
||||
}
|
||||
std::sort(outputs.begin(), outputs.end());
|
||||
for (const auto output : outputs) {
|
||||
const auto& slot = timing.by_output_pos.at(output);
|
||||
std::printf(" tx o%u: segment=%zu slot=%zu slot_count=%zu\n",
|
||||
output, slot.segment_index, slot.slot, slot.slot_count);
|
||||
}
|
||||
}
|
||||
|
||||
void print_motion_phase(const FrameTiming& timing, MotionMode motion_mode, const char* label) {
|
||||
for (std::uint32_t output = 0U; output < 4U; ++output) {
|
||||
auto trace = make_trace(output);
|
||||
const auto& slot = timing.by_output_pos.at(output);
|
||||
apply_motion_correction(
|
||||
trace,
|
||||
timing.segments[slot.segment_index],
|
||||
slot.slot,
|
||||
slot.slot_count,
|
||||
timing.frame_center_s,
|
||||
1.5e9,
|
||||
6.0e9,
|
||||
kSpeedOfLightMetersPerSec,
|
||||
kSpeedMps,
|
||||
kLookAngleDeg,
|
||||
kDirectionSign,
|
||||
motion_mode
|
||||
);
|
||||
// s21 started at 1+0j, so arg(s21[k]) = -phi[k] (notebook sign = -1).
|
||||
std::printf("%s tx o%u: phi(k=0)=%.12e phi(k=5)=%.12e phi(k=10)=%.12e\n",
|
||||
label, output,
|
||||
-std::arg(trace.s21[0]), -std::arg(trace.s21[5]), -std::arg(trace.s21[10]));
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto run_check() -> int {
|
||||
const auto run_config = make_run_config();
|
||||
|
||||
const auto timed = build_frame_timing(run_config, make_collection(true), 2U, true);
|
||||
print_timing("segmented+timestamps", timed);
|
||||
print_motion_phase(timed, MotionMode::IntMinus, "int_minus");
|
||||
print_motion_phase(timed, MotionMode::IntFocus, "int_focus");
|
||||
|
||||
const auto fallback = build_frame_timing(run_config, make_collection(false), 2U, true);
|
||||
print_timing("segmented+fallback", fallback);
|
||||
print_motion_phase(fallback, MotionMode::IntMinus, "fallback int_minus");
|
||||
|
||||
const auto interleaved = build_frame_timing(run_config, make_collection(true), 2U, false);
|
||||
print_timing("interleaved", interleaved);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace radar::processing
|
||||
|
||||
auto main() -> int {
|
||||
return radar::processing::run_check();
|
||||
}
|
||||
Reference in New Issue
Block a user