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

sar3d: port the Multi_antenna_3D_clean_v1 notebook algorithm

Browse Source 
The old sar3d core was a port of the superseded Horns_3D notebook and
produced wrong volumes. The compute core now follows the reference
notebook exactly:

- segmented_interleaved frame timing rebuilt from per-trace capture
  stamps (uniform tx_per_segment split as the no-stamps fallback), with
  the coarse inter-segment dtau and the intra-sweep dtau applied as one
  phase ramp before the IFFT;
- ground scene coordinates with the ground_depth_m lift (scene Y=0 is
  the ground), full-region grid instead of a window around a center
  depth, geo_ref compensation anchored on the ground plane, power 0.3;
- notebook finalization in C++: |acc/cnt|, depth mask, normalization
  without the Y-edge planes, separable Gaussian over (Z, X), level;
- CUDA kernel computes geometry in double: float32 loses ~3e-4 of an
  A-scan sample in tau and drifts ~1e-4 from the float64 reference.

Live-config keys follow: the center_depth/half_width/grid_n window knobs
are replaced by ground/voxel/y-range/margin/sigma/power/tx_per_segment.

Parity against the notebook npz exports (frames 0034-0036 of the
20260922_mine_fast_true dataset): max|diff| <= 1.6e-6 off the depth-mask
knife edge, identical argmax, level rel diff <= 7e-7. Verified via
make sar3d_parity --dump-dir + scripts/sar3d_volume_compare.py; the
executable spec lives in scripts/sar3d_reference_check.py (bit-tight
against the npz, including the animation shifted-grid arithmetic).

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
awe
2026-09-25 16:42:01 +03:00
co-authored by Claude Fable 5
parent e9a0b43ed3
commit b8cb572bc4
17 changed files with 1264 additions and 353 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp
+33 -13
View File
@@ -90,21 +90,41 @@ struct ProcessingLiveConfig {
float gpr_visible_x_max_m = 2.0F;
float gpr_visible_z_min_m = 0.0F;
float gpr_visible_z_max_m = 14.0F;
// 3D SAR (sar3d processor) parameters; defaults mirror VNA_AlGo gpr_core.
// Frequency band, depth gate, antenna selection and motion (speed, look
// angle, direction, phase correction) are shared with the gpr_* fields.
std::uint32_t sar3d_frame_radius = 6U;
float sar3d_edge_frame_weight = 0.75F;
float sar3d_pitch_deg = 6.0F;
float sar3d_center_depth_m = 5.6F;
float sar3d_half_width_x_m = 1.8F;
float sar3d_half_width_y_m = 1.0F;
float sar3d_half_width_z_m = 1.8F;
std::uint32_t sar3d_grid_nx = 150U;
std::uint32_t sar3d_grid_ny = 120U;
std::uint32_t sar3d_grid_nz = 160U;
// 3D SAR (sar3d processor) parameters; defaults mirror the reference
// notebook Multi_antenna_3D_clean_v1.ipynb (cells 0/4). Frequency band,
// depth gate, antenna selection and motion (speed, look angle, direction,
// phase correction) are shared with the gpr_* fields.
std::uint32_t sar3d_frame_radius = 0U;
float sar3d_edge_frame_weight = 0.8F;
// Frame pitch for ground coordinates and inter-frame offsets. Override
// disabled -> the pitch follows gpr_look_angle_deg (notebook
// BP_GROUND_PITCH_DEG = None).
bool sar3d_pitch_override_enabled = false;
float sar3d_pitch_deg = 17.5F;
// Height of the LOWER antenna row above the ground; scene Y=0 is the ground.
float sar3d_ground_depth_m = 1.25F;
// Full-region grid: X spans the antennas +- x_margin, Y is height above the
// ground, Z covers radar depths [gpr_min_depth, gpr_max_depth] over the Y
// range. Voxel pitch per axis.
float sar3d_x_margin_m = 2.0F;
float sar3d_y_min_m = -0.5F;
float sar3d_y_max_m = 2.0F;
float sar3d_voxel_x_m = 0.025F;
float sar3d_voxel_y_m = 0.02F;
float sar3d_voxel_z_m = 0.02F;
// Volume finalization: normalization ignores this much of the grid's Y
// edges (constant-range ridge), then Gaussian smoothing over (Z, X).
float sar3d_norm_edge_margin_m = 0.3F;
float sar3d_smooth_sigma = 1.5F;
// Geometric spreading compensation 1/(Rtx*Rrx)^power, capped.
float sar3d_comp_range_power = 0.3F;
// Fallback frame segmentation when per-trace capture stamps are absent.
std::uint32_t sar3d_tx_per_segment = 2U;
bool sar3d_use_compensation = true;
bool sar3d_normalize_pairs = true;
// Debug: when non-empty, the processor dumps every finished volume to this
// directory (raw float32 + json header). Used by the parity tool only.
std::string sar3d_volume_dump_dir{};
// Bottom-view projection angle around the vertical Y axis, degrees:
// 0 = front view (X horizontal), 90 = side view (Z horizontal),
// intermediate values project along the rotated horizontal direction.
data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp
+23 -20
View File
@@ -392,10 +392,16 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
for (const auto& [key, target] : {
std::pair{"sar3d_edge_frame_weight", &config.sar3d_edge_frame_weight},
std::pair{"sar3d_pitch_deg", &config.sar3d_pitch_deg},
std::pair{"sar3d_center_depth_m", &config.sar3d_center_depth_m},
std::pair{"sar3d_half_width_x_m", &config.sar3d_half_width_x_m},
std::pair{"sar3d_half_width_y_m", &config.sar3d_half_width_y_m},
std::pair{"sar3d_half_width_z_m", &config.sar3d_half_width_z_m},
std::pair{"sar3d_ground_depth_m", &config.sar3d_ground_depth_m},
std::pair{"sar3d_x_margin_m", &config.sar3d_x_margin_m},
std::pair{"sar3d_y_min_m", &config.sar3d_y_min_m},
std::pair{"sar3d_y_max_m", &config.sar3d_y_max_m},
std::pair{"sar3d_voxel_x_m", &config.sar3d_voxel_x_m},
std::pair{"sar3d_voxel_y_m", &config.sar3d_voxel_y_m},
std::pair{"sar3d_voxel_z_m", &config.sar3d_voxel_z_m},
std::pair{"sar3d_norm_edge_margin_m", &config.sar3d_norm_edge_margin_m},
std::pair{"sar3d_smooth_sigma", &config.sar3d_smooth_sigma},
std::pair{"sar3d_comp_range_power", &config.sar3d_comp_range_power},
std::pair{"sar3d_bottom_view_deg", &config.sar3d_bottom_view_deg},
}) {
if (const auto found = root.find(key); found != root.end()) {
@@ -405,27 +411,24 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
*target = static_cast<float>(found->get<double>());
}
}
if (const auto found = root.find("sar3d_tx_per_segment"); found != root.end()) {
config.sar3d_tx_per_segment = parse_u32_number(*found, "processing.sar3d_tx_per_segment");
if (config.sar3d_tx_per_segment == 0U) {
throw std::runtime_error("processing.sar3d_tx_per_segment must be >= 1");
}
}
for (const auto& [key, target] : {
std::pair{"sar3d_grid_nx", &config.sar3d_grid_nx},
std::pair{"sar3d_grid_ny", &config.sar3d_grid_ny},
std::pair{"sar3d_grid_nz", &config.sar3d_grid_nz},
std::pair{"sar3d_pitch_override_enabled", &config.sar3d_pitch_override_enabled},
std::pair{"sar3d_use_compensation", &config.sar3d_use_compensation},
std::pair{"sar3d_normalize_pairs", &config.sar3d_normalize_pairs},
}) {
if (const auto found = root.find(key); found != root.end()) {
*target = parse_u32_number(*found, std::string("processing.") + key);
if (!found->is_boolean()) {
throw std::runtime_error(std::string("processing.") + key + " must be bool");
}
*target = found->get<bool>();
}
}
if (const auto found = root.find("sar3d_use_compensation"); found != root.end()) {
if (!found->is_boolean()) {
throw std::runtime_error("processing.sar3d_use_compensation must be bool");
}
config.sar3d_use_compensation = found->get<bool>();
}
if (const auto found = root.find("sar3d_normalize_pairs"); found != root.end()) {
if (!found->is_boolean()) {
throw std::runtime_error("processing.sar3d_normalize_pairs must be bool");
}
config.sar3d_normalize_pairs = found->get<bool>();
}
if (const auto found = root.find("ignore_socket_speed"); found != root.end()) {
if (!found->is_boolean()) {
throw std::runtime_error("processing.ignore_socket_speed must be bool");
data_acq_and_processing/processing/processors/src/sar3d_processor.cpp
+161 -55
View File
@@ -4,7 +4,10 @@
#include <bit>
#include <cmath>
#include <cstdint>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <optional>
#include <stdexcept>
@@ -19,7 +22,6 @@ namespace radar::processing {
namespace {
constexpr std::size_t kAscanCacheLimit = 64U;
constexpr double kVolumeEps = 1e-12;
void hash_mix(std::size_t& seed, std::uint64_t value) {
seed ^= value + 0x9e3779b97f4a7c15ULL + (seed << 6U) + (seed >> 2U);
@@ -130,6 +132,7 @@ struct Selection {
bp.pair_normalize = live_config.sar3d_normalize_pairs;
bp.pair_norm_depth_min = bp.min_depth;
bp.pair_norm_depth_max = bp.max_depth;
bp.comp_range_power = static_cast<double>(live_config.sar3d_comp_range_power);
return bp;
}
@@ -137,9 +140,13 @@ struct Selection {
gpr::MotionConfig motion{};
motion.speed_m_s = static_cast<double>(live_config.gpr_speed_m_s);
motion.look_angle_deg = static_cast<double>(live_config.gpr_look_angle_deg);
motion.pitch_deg = static_cast<double>(live_config.sar3d_pitch_deg);
// Notebook BP_GROUND_PITCH_DEG = None -> the frame pitch follows the look angle.
motion.pitch_deg = live_config.sar3d_pitch_override_enabled
? static_cast<double>(live_config.sar3d_pitch_deg)
: static_cast<double>(live_config.gpr_look_angle_deg);
motion.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
motion.apply_freq_phase_correction = live_config.gpr_apply_freq_phase_correction;
motion.tx_per_segment = static_cast<int>(std::max<std::uint32_t>(1U, live_config.sar3d_tx_per_segment));
return motion;
}
@@ -147,20 +154,35 @@ struct Selection {
gpr::SARConfig sar{};
sar.frame_radius = static_cast<int>(std::max<std::uint32_t>(0U, live_config.sar3d_frame_radius));
sar.edge_frame_weight = static_cast<double>(live_config.sar3d_edge_frame_weight);
sar.half_width_m = {
static_cast<double>(live_config.sar3d_half_width_x_m),
static_cast<double>(live_config.sar3d_half_width_y_m),
static_cast<double>(live_config.sar3d_half_width_z_m),
};
sar.grid = {
static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_nx)),
static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_ny)),
static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_nz)),
sar.ground_depth_m = static_cast<double>(live_config.sar3d_ground_depth_m);
sar.x_margin_m = static_cast<double>(live_config.sar3d_x_margin_m);
sar.y_min_m = static_cast<double>(live_config.sar3d_y_min_m);
sar.y_max_m = static_cast<double>(live_config.sar3d_y_max_m);
sar.voxel_m = {
static_cast<double>(live_config.sar3d_voxel_x_m),
static_cast<double>(live_config.sar3d_voxel_y_m),
static_cast<double>(live_config.sar3d_voxel_z_m),
};
sar.norm_edge_margin_m = static_cast<double>(live_config.sar3d_norm_edge_margin_m);
sar.smooth_sigma = static_cast<double>(live_config.sar3d_smooth_sigma);
sar.use_compensation = live_config.sar3d_use_compensation;
return sar;
}
// Hardware Tx order: first occurrence of each output in run_combos. Deliberately
// ignores the live Tx selection so segment/slot assignment stays a hardware
// property (notebook tx_order_full).
[[nodiscard]] auto tx_order_full(const config::RunConfig& run_config) -> std::vector<int> {
std::vector<int> order{};
for (const auto& combo : run_config.run_combos) {
const int output = static_cast<int>(combo.output_pos);
if (std::find(order.begin(), order.end(), output) == order.end()) {
order.push_back(output);
}
}
return order;
}
[[nodiscard]] auto ascan_fingerprint(
const Selection& selection,
const gpr::GeometryConfig& geom,
@@ -184,6 +206,7 @@ struct Selection {
hash_mix(seed, motion.look_angle_deg);
hash_mix(seed, motion.direction_sign);
hash_mix(seed, static_cast<std::uint64_t>(motion.apply_freq_phase_correction ? 1U : 0U));
hash_mix(seed, static_cast<std::uint64_t>(motion.tx_per_segment));
return seed;
}
@@ -215,6 +238,7 @@ struct Selection {
hash_mix(seed, bp.comp_weight_max);
hash_mix(seed, bp.comp_ref_depth);
hash_mix(seed, motion.pitch_deg);
hash_mix(seed, sar.ground_depth_m);
hash_mix(seed, static_cast<std::uint64_t>(sar.use_compensation ? 1U : 0U));
return seed;
}
@@ -228,14 +252,80 @@ struct Selection {
return static_cast<double>(collection.monotonic_ns) * 1e-9;
}
// One Tx's interleaved sweep spans the whole capture window, so the per-Tx
// sweep time is capture_span / N_tx (same derivation as the coherent BP path).
[[nodiscard]] auto tx_sweep_time_s(const ipc::PreprocessedCollection& collection, int n_tx) -> double {
if (collection.capture_end_ns <= collection.capture_start_ns || n_tx <= 0) {
return 0.0;
// Real frame timing from the per-trace capture stamps: traces measured in one
// segment share one window on both producers, so the first window seen per Tx
// reconstructs the segments exactly (notebook snapshot_acquisition_timing).
// Invalid when any trace carries the zero "unmeasured" sentinel.
struct SnapshotTiming {
gpr::TxWindows windows{}; // seconds relative to the first trace start
double acq_s = 0.0;
double center_abs_s = 0.0; // absolute acquisition centre (frame time)
bool valid = false;
};
[[nodiscard]] auto snapshot_timing(const ipc::PreprocessedCollection& collection) -> SnapshotTiming {
SnapshotTiming timing{};
if (collection.traces.empty()) {
return timing;
}
const double span_s = static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9;
return span_s / static_cast<double>(n_tx);
std::map<int, std::pair<std::uint64_t, std::uint64_t>> by_tx{};
std::uint64_t first = std::numeric_limits<std::uint64_t>::max();
std::uint64_t last = 0U;
for (const auto& trace : collection.traces) {
if (trace.capture_end_ns <= trace.capture_start_ns) {
return timing;
}
by_tx.try_emplace(
static_cast<int>(trace.combo.output_pos),
std::pair{trace.capture_start_ns, trace.capture_end_ns}
);
first = std::min(first, trace.capture_start_ns);
last = std::max(last, trace.capture_end_ns);
}
for (const auto& [tx, window] : by_tx) {
timing.windows[tx] = {
static_cast<double>(window.first - first) * 1e-9,
static_cast<double>(window.second - first) * 1e-9,
};
}
timing.acq_s = static_cast<double>(last - first) * 1e-9;
timing.center_abs_s = 0.5 * (static_cast<double>(first) + static_cast<double>(last)) * 1e-9;
timing.valid = true;
return timing;
}
// Debug dump for the parity tool: raw float32 volume [iy][iz][ix] plus a json
// header with the axes and level.
void dump_volume(
const std::string& dir,
std::uint64_t collection_id,
const gpr::FinishedVolume& finished,
const gpr::Grid& grid
) {
char name[64];
std::snprintf(name, sizeof(name), "/sar3d_volume_%llu",
static_cast<unsigned long long>(collection_id));
const std::string base = dir + name;
std::ofstream bin(base + ".f32", std::ios::binary | std::ios::trunc);
bin.write(
reinterpret_cast<const char*>(finished.volume.data()),
static_cast<std::streamsize>(finished.volume.size() * sizeof(float))
);
std::ofstream meta(base + ".json", std::ios::trunc);
meta.precision(17);
meta << "{\"nx\": " << finished.NX << ", \"ny\": " << finished.NY
<< ", \"nz\": " << finished.NZ << ", \"level\": " << finished.level;
const auto write_axis = [&meta](const char* key, const std::vector<double>& axis) {
meta << ", \"" << key << "\": [";
for (std::size_t index = 0U; index < axis.size(); ++index) {
meta << (index > 0U ? "," : "") << axis[index];
}
meta << "]";
};
write_axis("x_m", grid.x);
write_axis("y_m", grid.y);
write_axis("z_m", grid.z);
meta << "}\n";
}
[[nodiscard]] auto build_image_payload(
@@ -285,14 +375,26 @@ struct Sar3dProcessor::State {
const Selection& selection,
const gpr::GeometryConfig& geom,
const gpr::BPConfig& bp,
gpr::MotionConfig motion
const gpr::MotionConfig& motion,
const std::vector<int>& tx_order
) -> const CacheEntry& {
if (const auto found = ascan_cache.find(collection.collection_id); found != ascan_cache.end()) {
return found->second;
}
CacheEntry entry{};
entry.time_s = collection_time_s(collection);
auto timing = snapshot_timing(collection);
// The segmented model needs a window for every selected Tx; fall back to
// the uniform split otherwise (notebook median model).
if (timing.valid) {
for (const auto output : geom.output_idx) {
if (timing.windows.find(output) == timing.windows.end()) {
timing.valid = false;
break;
}
}
}
entry.time_s = timing.valid ? timing.center_abs_s : collection_time_s(collection);
std::map<gpr::PairKey, std::vector<gpr::c128>> s21{};
std::map<gpr::PairKey, std::vector<double>> freq{};
@@ -317,9 +419,25 @@ struct Sar3dProcessor::State {
}
if (s21.size() == selection.inputs.size() * selection.outputs.size()) {
motion.tx_sweep_time_s = tx_sweep_time_s(collection, geom.n_tx);
const auto timestamps = gpr::compute_pair_timestamps(motion, geom);
gpr::TxWindows windows{};
double acq_s = 0.0;
if (timing.valid) {
windows = std::move(timing.windows);
acq_s = timing.acq_s;
} else {
if (collection.capture_end_ns > collection.capture_start_ns) {
acq_s = static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9;
}
auto order = tx_order;
for (const auto output : geom.output_idx) {
if (std::find(order.begin(), order.end(), output) == order.end()) {
order.push_back(output);
}
}
windows = gpr::uniform_tx_windows(order, acq_s, motion.tx_per_segment);
}
try {
const auto timestamps = gpr::compute_pair_timestamps(windows, acq_s, tx_order, geom, motion);
entry.ascans = std::make_shared<gpr::AScanSet>(gpr::preprocess_snapshot(
s21, freq, /*background=*/nullptr, motion, geom, bp, timestamps, bp.pair_normalize
));
@@ -399,12 +517,14 @@ auto Sar3dProcessor::process_collection(
}
window.push_back(&collection);
const auto tx_order = tx_order_full(run_config);
std::vector<std::shared_ptr<gpr::AScanSet>> ascan_sets{};
std::vector<double> frame_times{};
ascan_sets.reserve(window.size());
frame_times.reserve(window.size());
for (const auto* entry_collection : window) {
const auto& entry = state_->get_or_compute_ascans(*entry_collection, selection, geom, bp, motion);
const auto& entry =
state_->get_or_compute_ascans(*entry_collection, selection, geom, bp, motion, tx_order);
if (entry.ascans == nullptr) {
continue;
}
@@ -450,14 +570,17 @@ auto Sar3dProcessor::process_collection(
state_->engine_fp = engine_fp;
}
const auto center_scene =
gpr::point_to_scene(0.0, 0.0, static_cast<double>(live_config.sar3d_center_depth_m), motion, sar);
const gpr::Grid grid = gpr::build_grid_scene(center_scene, sar);
const gpr::VolumeRaw volume = state_->engine->aperture_volume(frames, grid);
const gpr::Grid grid = gpr::build_full_region_grid(geom, bp, sar, motion.pitch_deg);
const gpr::VolumeRaw raw = state_->engine->aperture_volume(frames, grid);
// Full notebook finalization: |acc/cnt|, depth gate, normalization
// without the Y-edge planes, Gaussian smoothing over (Z, X), clip.
const gpr::FinishedVolume finished =
gpr::finish_volume(raw, grid, motion.pitch_deg, bp, sar);
if (!live_config.sar3d_volume_dump_dir.empty()) {
dump_volume(live_config.sar3d_volume_dump_dir, collection.collection_id, finished, grid);
}
// Finalization (port of gpr_core sar_finish_volume, minus smoothing):
// |acc/(cnt+eps)|, ground-coordinate depth gate, normalization to peak.
//
// Two projections match the legacy render conventions (X across,
// Z range/"вдаль", Y up):
// - top view `sar3d_proj_xz`: max over Y, axes X (horizontal) / Z;
@@ -466,12 +589,9 @@ auto Sar3dProcessor::process_collection(
// `sar3d_bottom_view_deg` around Y: 0° = front view (u = X, max
// along Z), 90° = side view (u = Z, max along X), intermediate
// angles project along the rotated perpendicular.
const int nx = volume.NX;
const int ny = volume.NY;
const int nz = volume.NZ;
const double pitch_rad = motion.pitch_deg * M_PI / 180.0;
const double cos_p = std::cos(pitch_rad);
const double sin_p = std::sin(pitch_rad);
const int nx = finished.NX;
const int ny = finished.NY;
const int nz = finished.NZ;
const double theta_deg = std::clamp(static_cast<double>(live_config.sar3d_bottom_view_deg), 0.0, 90.0);
const double theta_rad = theta_deg * M_PI / 180.0;
@@ -501,24 +621,18 @@ auto Sar3dProcessor::process_collection(
const double u_start = u_axis.front();
const double inv_du = nu > 1 ? static_cast<double>(nu - 1) / (u_axis.back() - u_start) : 0.0;
// The volume is already normalized to ~[0, 1] by finish_volume, so the
// projections are published as-is; the scalar payload carries `level`
// (the absolute scale the volume was divided by).
std::vector<float> projection_xz(static_cast<std::size_t>(nz) * nx, 0.0F);
std::vector<float> projection_bottom(static_cast<std::size_t>(ny) * nu, 0.0F);
float peak = 0.0F;
for (int iy = 0; iy < ny; ++iy) {
for (int iz = 0; iz < nz; ++iz) {
const double z_rad = -grid.y[iy] * sin_p + grid.z[iz] * cos_p;
const bool depth_ok = z_rad >= bp.min_depth && z_rad <= bp.max_depth;
if (!depth_ok) {
continue;
}
const std::size_t row = (static_cast<std::size_t>(iy) * nz + iz) * nx;
const std::size_t bottom_row = static_cast<std::size_t>(iy) * nu;
const double z_sin_t = grid.z[iz] * sin_t;
for (int ix = 0; ix < nx; ++ix) {
const std::size_t voxel = row + static_cast<std::size_t>(ix);
const float value = std::abs(volume.acc[voxel]) /
(volume.cnt[voxel] + static_cast<float>(kVolumeEps));
peak = std::max(peak, value);
const float value = finished.volume[row + static_cast<std::size_t>(ix)];
auto& xz_cell = projection_xz[static_cast<std::size_t>(iz) * nx + ix];
xz_cell = std::max(xz_cell, value);
int bin = ix;
@@ -534,14 +648,6 @@ auto Sar3dProcessor::process_collection(
}
}
}
if (peak > 0.0F) {
for (auto& value : projection_xz) {
value /= peak;
}
for (auto& value : projection_bottom) {
value /= peak;
}
}
results.collection_payloads.push_back(
build_image_payload("sar3d_proj_xz", grid.x, grid.z, std::move(projection_xz))
@@ -552,7 +658,7 @@ auto Sar3dProcessor::process_collection(
ipc::ResultPayload peak_payload{};
peak_payload.processing_name = "sar3d_peak";
peak_payload.kind = ipc::ResultKind::ScalarF32;
peak_payload.scalar_value = peak;
peak_payload.scalar_value = static_cast<float>(finished.level);
results.collection_payloads.push_back(std::move(peak_payload));
} catch (const std::exception& exc) {
// A CUDA/NVRTC or data error must not kill the data_processor loop:
data_acq_and_processing/processing/tools/sar3d_parity_main.cpp
+113 -80
View File
@@ -1,12 +1,18 @@
// Parity check for the sar3d processor against the VNA_AlGo reference path.
// Parity check of the sar3d processor against the reference notebook
// Multi_antenna_3D_clean_v1.ipynb.
//
// Loads the same .npy snapshot dataset the gpr_cpp `standalone_bp` example
// uses, feeds it through Sar3dProcessor (combo mapping, timestamps, A-scan
// cache, aperture, finalization) and prints the volume peak and the argmax of
// both projections. Compare against `standalone_bp <dir> <center>` output:
// the peak |acc/cnt| and the (X, Y, Z) of the maximum voxel must match.
// Feeds a preprocessed snapshot of an extracted replay dataset through
// Sar3dProcessor exactly the way the live pipeline would (geometry and combo
// order from the dataset's config_profile.json, per-trace capture windows from
// the snapshot's meta.json) and dumps the finished volume for comparison with
// the notebook's *_volume.npz export:
//
// Usage: sar3d_parity <preprocessed_dir> <center_snapshot_name>
// sar3d_parity <dataset_root> <center_snapshot> [--dump-dir <dir>]
// python_app/scripts/sar3d_volume_compare.py <dir>/sar3d_volume_<id> <ref.npz>
//
// <dataset_root> contains config_profile.json and preprocessed/<snapshot>/.
// Defaults mirror the parameters the reference volumes were built with
// (f 2..6 GHz, depth 2.5..8 m, speed 1.45 m/s, look angle 17.5 deg).
#include <algorithm>
#include <cctype>
#include <complex>
@@ -19,6 +25,8 @@
#include <string>
#include <vector>
#include <nlohmann/json.hpp>
#include "processing_live_config.hpp"
#include "run_config.hpp"
#include "sar3d_processor.hpp"
@@ -26,8 +34,7 @@
namespace {
// Minimal numpy .npy loader (v1.0, 1-D complex128/complex64/float64/float32),
// mirroring gpr_cpp/examples/standalone_bp.cpp.
// Minimal numpy .npy loader (v1.0, 1-D complex128/complex64/float64/float32).
struct NpyArray {
std::vector<double> f64{};
std::vector<std::complex<double>> c128{};
@@ -77,43 +84,23 @@ NpyArray load_npy(const std::string& path) {
return array;
}
// VNA_AlGo reference geometry (gpr_core/config.py) keyed by switch positions.
auto build_run_config() -> radar::config::RunConfig {
radar::config::RunConfig config{};
config.gpr.relative_permittivity = 1.0F;
config.gpr.tx_geometry = {
{.output_pos = 0U, .x_m = 0.785F, .y_m = 0.0F, .z_m = 0.0F},
{.output_pos = 1U, .x_m = -0.785F, .y_m = 0.0F, .z_m = 0.0F},
};
config.gpr.rx_geometry = {
{.input_pos = 0U, .x_m = -0.42F, .y_m = 0.0F, .z_m = 0.0F},
{.input_pos = 1U, .x_m = 0.48F, .y_m = 0.0F, .z_m = 0.0F},
{.input_pos = 2U, .x_m = 0.18F, .y_m = 0.0F, .z_m = 0.0F},
{.input_pos = 3U, .x_m = -0.12F, .y_m = 0.0F, .z_m = 0.0F},
};
return config;
}
// Live config matching the VNA_AlGo defaults (BPConfig/MotionConfig/SARConfig).
// Live config matching the reference notebook run (npz meta parameters).
auto build_live_config() -> radar::processing::ProcessingLiveConfig {
radar::processing::ProcessingLiveConfig live{};
live.processor_mode = "sar3d";
live.gpr_start_freq_mhz = 2000.0F;
live.gpr_stop_freq_mhz = 6000.0F;
live.gpr_min_depth_m = 3.2F;
live.gpr_max_depth_m = 10.0F;
live.gpr_speed_m_s = 0.854F;
live.gpr_look_angle_deg = 6.0F;
live.gpr_min_depth_m = 2.5F;
live.gpr_max_depth_m = 8.0F;
live.gpr_speed_m_s = 1.45F;
live.gpr_look_angle_deg = 17.5F;
live.gpr_direction_sign = 1.0F;
live.gpr_apply_freq_phase_correction = true;
return live;
}
constexpr double kTxSweepTimeS = 0.079;
constexpr int kTxCount = 2;
constexpr int kRxCount = 4;
auto load_collection(
const radar::config::RunConfig& run_config,
const std::string& root,
const std::string& snapshot,
std::uint64_t collection_id
@@ -121,46 +108,73 @@ auto load_collection(
radar::ipc::PreprocessedCollection collection{};
collection.collection_id = collection_id;
// Frame time from the `_ns<...>` folder suffix, identical to standalone_bp.
const auto ns_pos = snapshot.rfind("_ns");
if (ns_pos == std::string::npos) {
throw std::runtime_error("snapshot has no _ns suffix: " + snapshot);
}
const auto mid_ns = static_cast<std::uint64_t>(std::stoull(snapshot.substr(ns_pos + 3)));
// The processor derives tx_sweep_time from capture_span / n_tx and the frame
// time from the capture midpoint, so set the window symmetric around mid_ns.
const auto span_ns = static_cast<std::uint64_t>(kTxCount * kTxSweepTimeS * 1e9);
collection.capture_start_ns = mid_ns - span_ns / 2U;
collection.capture_end_ns = mid_ns + span_ns / 2U;
collection.monotonic_ns = mid_ns;
for (int rx = 0; rx < kRxCount; ++rx) {
for (int tx = 0; tx < kTxCount; ++tx) {
char path[512];
std::snprintf(path, sizeof(path), "%s/%s/i%d_o%d_s21.npy", root.c_str(), snapshot.c_str(), rx, tx);
const auto s21 = load_npy(path);
std::snprintf(path, sizeof(path), "%s/%s/i%d_o%d_freq.npy", root.c_str(), snapshot.c_str(), rx, tx);
const auto freq = load_npy(path);
radar::ipc::SweepTraceBlock trace{};
trace.combo = radar::ipc::ComboKey{
.input_pos = static_cast<std::uint32_t>(rx),
.output_pos = static_cast<std::uint32_t>(tx),
};
trace.frequency_hz.assign(freq.f64.begin(), freq.f64.end());
trace.s21.reserve(s21.c128.size());
for (const auto& sample : s21.c128) {
trace.s21.push_back(radar::ipc::Complex32{
.re = static_cast<float>(sample.real()),
.im = static_cast<float>(sample.imag()),
});
// Per-trace capture windows from the snapshot's meta.json; the segmented
// timing model is reconstructed from them exactly like in the notebook.
std::map<std::pair<std::uint32_t, std::uint32_t>, std::pair<std::uint64_t, std::uint64_t>> windows{};
{
std::ifstream in(root + "/preprocessed/" + snapshot + "/meta.json");
if (in) {
nlohmann::json meta{};
in >> meta;
for (const auto& trace : meta.value("traces", nlohmann::json::array())) {
if (!trace.contains("capture_start_ns") || !trace.contains("capture_end_ns")) {
continue;
}
windows[{trace.at("input").get<std::uint32_t>(), trace.at("output").get<std::uint32_t>()}] = {
trace.at("capture_start_ns").get<std::uint64_t>(),
trace.at("capture_end_ns").get<std::uint64_t>(),
};
}
trace.s11.resize(trace.s21.size());
trace.capture_start_ns = collection.capture_start_ns;
trace.capture_end_ns = collection.capture_end_ns;
collection.traces.push_back(std::move(trace));
}
}
std::uint64_t first = 0U;
std::uint64_t last = 0U;
for (const auto& [key, window] : windows) {
if (first == 0U || window.first < first) {
first = window.first;
}
last = std::max(last, window.second);
}
if (first == 0U) {
// No meta labels: fall back to the `_ns<...>` folder suffix.
const auto ns_pos = snapshot.rfind("_ns");
if (ns_pos == std::string::npos) {
throw std::runtime_error("snapshot has no meta.json and no _ns suffix: " + snapshot);
}
last = static_cast<std::uint64_t>(std::stoull(snapshot.substr(ns_pos + 3)));
first = last;
}
collection.capture_start_ns = first;
collection.capture_end_ns = last;
collection.monotonic_ns = (first + last) / 2U;
for (const auto& combo : run_config.run_combos) {
char path[512];
std::snprintf(path, sizeof(path), "%s/preprocessed/%s/i%u_o%u_s21.npy",
root.c_str(), snapshot.c_str(), combo.input_pos, combo.output_pos);
const auto s21 = load_npy(path);
std::snprintf(path, sizeof(path), "%s/preprocessed/%s/i%u_o%u_freq.npy",
root.c_str(), snapshot.c_str(), combo.input_pos, combo.output_pos);
const auto freq = load_npy(path);
radar::ipc::SweepTraceBlock trace{};
trace.combo = combo;
trace.frequency_hz.assign(freq.f64.begin(), freq.f64.end());
trace.s21.reserve(s21.c128.size());
for (const auto& sample : s21.c128) {
trace.s21.push_back(radar::ipc::Complex32{
.re = static_cast<float>(sample.real()),
.im = static_cast<float>(sample.imag()),
});
}
trace.s11.resize(trace.s21.size());
if (const auto found = windows.find({combo.input_pos, combo.output_pos}); found != windows.end()) {
trace.capture_start_ns = found->second.first;
trace.capture_end_ns = found->second.second;
}
collection.traces.push_back(std::move(trace));
}
return collection;
}
@@ -195,14 +209,21 @@ void print_projection_argmax(
int main(int argc, char** argv) {
if (argc < 3) {
std::printf("usage: %s <preprocessed_dir> <center_snapshot>\n", argv[0]);
std::printf("usage: %s <dataset_root> <center_snapshot> [--dump-dir <dir>]\n", argv[0]);
return 1;
}
const std::string root = argv[1];
const std::string center = argv[2];
std::string dump_dir{};
for (int index = 3; index + 1 < argc; ++index) {
if (std::string{argv[index]} == "--dump-dir") {
dump_dir = argv[index + 1];
}
}
const std::string preprocessed = root + "/preprocessed";
std::vector<std::string> snapshots{};
if (DIR* dir = opendir(root.c_str())) {
if (DIR* dir = opendir(preprocessed.c_str())) {
for (dirent* entry = nullptr; (entry = readdir(dir)) != nullptr;) {
const std::string name = entry->d_name;
if (!name.empty() && (std::isdigit(name[0]) != 0)) {
@@ -214,28 +235,34 @@ int main(int argc, char** argv) {
std::sort(snapshots.begin(), snapshots.end());
const auto center_it = std::find(snapshots.begin(), snapshots.end(), center);
if (center_it == snapshots.end()) {
std::printf("center %s not found\n", center.c_str());
std::printf("center %s not found in %s\n", center.c_str(), preprocessed.c_str());
return 1;
}
const auto center_index = static_cast<std::size_t>(center_it - snapshots.begin());
const auto live = build_live_config();
auto live = build_live_config();
live.sar3d_volume_dump_dir = dump_dir;
const auto radius = static_cast<std::size_t>(live.sar3d_frame_radius);
if (center_index < radius || center_index + radius >= snapshots.size()) {
std::printf("not enough snapshots around center for radius %zu\n", radius);
return 1;
}
const auto run_config = radar::config::load_run_config(root + "/config_profile.json");
if (run_config.gpr.tx_geometry.empty() || run_config.gpr.rx_geometry.empty()) {
std::printf("FAIL: config_profile.json has no gpr geometry\n");
return 1;
}
// The processor centers the aperture `radius` frames before the newest
// collection, so feeding [center-r .. center+r] in order reproduces the
// symmetric aperture of standalone_bp around `center`.
// symmetric aperture around `center`.
std::vector<radar::ipc::PreprocessedCollection> history{};
for (std::size_t index = center_index - radius; index <= center_index + radius; ++index) {
history.push_back(load_collection(root, snapshots[index], index));
history.push_back(load_collection(run_config, root, snapshots[index], index));
}
std::printf("aperture: %zu frames around %s\n", history.size(), center.c_str());
const auto run_config = build_run_config();
radar::processing::Sar3dProcessor processor{};
radar::ipc::ResultCollection result{};
for (std::size_t index = 0U; index < history.size(); ++index) {
@@ -253,7 +280,7 @@ int main(int argc, char** argv) {
}
for (const auto& payload : result.collection_payloads) {
if (payload.kind == radar::ipc::ResultKind::ScalarF32) {
std::printf("%s: %.6e (compare with standalone_bp max |acc/cnt|)\n",
std::printf("%s (volume level): %.9e\n",
payload.processing_name.c_str(), static_cast<double>(payload.scalar_value));
}
}
@@ -269,6 +296,7 @@ int main(int argc, char** argv) {
std::span<const radar::ipc::PreprocessedCollection>(history.data(), history.size() - 1U);
for (const float angle_deg : {0.0F, 30.0F, 60.0F, 90.0F}) {
auto live_angle = live;
live_angle.sar3d_volume_dump_dir.clear();
live_angle.sar3d_bottom_view_deg = angle_deg;
const auto angle_result =
processor.process_collection(run_config, history.back(), last_previous, live_angle);
@@ -279,6 +307,11 @@ int main(int argc, char** argv) {
}
}
}
if (!dump_dir.empty()) {
std::printf("volume dumped to %s/sar3d_volume_%llu.{f32,json}\n",
dump_dir.c_str(),
static_cast<unsigned long long>(history.back().collection_id));
}
std::printf("OK: sar3d processor path completed\n");
return 0;
}