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/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;
}