projector version
This commit is contained in:
@@ -0,0 +1,263 @@
|
||||
// Parity check for the sar3d processor against the VNA_AlGo reference path.
|
||||
//
|
||||
// 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.
|
||||
//
|
||||
// Usage: sar3d_parity <preprocessed_dir> <center_snapshot_name>
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <dirent.h>
|
||||
#include <fstream>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "processing_live_config.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "sar3d_processor.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
// Minimal numpy .npy loader (v1.0, 1-D complex128/complex64/float64/float32),
|
||||
// mirroring gpr_cpp/examples/standalone_bp.cpp.
|
||||
struct NpyArray {
|
||||
std::vector<double> f64{};
|
||||
std::vector<std::complex<double>> c128{};
|
||||
};
|
||||
|
||||
NpyArray load_npy(const std::string& path) {
|
||||
std::ifstream in(path, std::ios::binary);
|
||||
if (!in) {
|
||||
throw std::runtime_error("cannot open " + path);
|
||||
}
|
||||
char magic[6];
|
||||
in.read(magic, 6);
|
||||
std::uint8_t version[2];
|
||||
in.read(reinterpret_cast<char*>(version), 2);
|
||||
std::uint16_t header_len = 0;
|
||||
in.read(reinterpret_cast<char*>(&header_len), 2);
|
||||
std::string header(header_len, '\0');
|
||||
in.read(header.data(), header_len);
|
||||
const auto shape_pos = header.find("'shape':");
|
||||
const auto left = header.find('(', shape_pos);
|
||||
const auto right = header.find(')', shape_pos);
|
||||
const long count = std::stol(header.substr(left + 1, right - left - 1));
|
||||
|
||||
NpyArray array{};
|
||||
const auto has = [&header](const char* token) { return header.find(token) != std::string::npos; };
|
||||
if (has("c8")) {
|
||||
array.c128.resize(count);
|
||||
std::vector<std::complex<float>> tmp(count);
|
||||
in.read(reinterpret_cast<char*>(tmp.data()), count * sizeof(std::complex<float>));
|
||||
for (long i = 0; i < count; ++i) {
|
||||
array.c128[i] = std::complex<double>(tmp[i].real(), tmp[i].imag());
|
||||
}
|
||||
} else if (has("c16")) {
|
||||
array.c128.resize(count);
|
||||
in.read(reinterpret_cast<char*>(array.c128.data()), count * sizeof(std::complex<double>));
|
||||
} else if (has("f4")) {
|
||||
array.f64.resize(count);
|
||||
std::vector<float> tmp(count);
|
||||
in.read(reinterpret_cast<char*>(tmp.data()), count * sizeof(float));
|
||||
for (long i = 0; i < count; ++i) {
|
||||
array.f64[i] = tmp[i];
|
||||
}
|
||||
} else {
|
||||
array.f64.resize(count);
|
||||
in.read(reinterpret_cast<char*>(array.f64.data()), count * sizeof(double));
|
||||
}
|
||||
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).
|
||||
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_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 std::string& root,
|
||||
const std::string& snapshot,
|
||||
std::uint64_t collection_id
|
||||
) -> radar::ipc::PreprocessedCollection {
|
||||
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()),
|
||||
});
|
||||
}
|
||||
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));
|
||||
}
|
||||
}
|
||||
return collection;
|
||||
}
|
||||
|
||||
void print_projection_argmax(const radar::ipc::ResultPayload& payload, const char* row_axis_name) {
|
||||
std::size_t argmax = 0U;
|
||||
float best = -1.0F;
|
||||
for (std::size_t index = 0U; index < payload.image_values.size(); ++index) {
|
||||
if (payload.image_values[index] > best) {
|
||||
best = payload.image_values[index];
|
||||
argmax = index;
|
||||
}
|
||||
}
|
||||
const auto nx = payload.image_x_axis.size();
|
||||
const auto col = argmax % nx;
|
||||
const auto row = argmax / nx;
|
||||
std::printf(
|
||||
"%s: max=%.6f at X=%.1f cm, %s=%.1f cm\n",
|
||||
payload.processing_name.c_str(),
|
||||
static_cast<double>(best),
|
||||
static_cast<double>(payload.image_x_axis[col]) * 100.0,
|
||||
row_axis_name,
|
||||
static_cast<double>(payload.image_y_axis[row]) * 100.0
|
||||
);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 3) {
|
||||
std::printf("usage: %s <preprocessed_dir> <center_snapshot>\n", argv[0]);
|
||||
return 1;
|
||||
}
|
||||
const std::string root = argv[1];
|
||||
const std::string center = argv[2];
|
||||
|
||||
std::vector<std::string> snapshots{};
|
||||
if (DIR* dir = opendir(root.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)) {
|
||||
snapshots.push_back(name);
|
||||
}
|
||||
}
|
||||
closedir(dir);
|
||||
}
|
||||
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());
|
||||
return 1;
|
||||
}
|
||||
const auto center_index = static_cast<std::size_t>(center_it - snapshots.begin());
|
||||
|
||||
const auto live = build_live_config();
|
||||
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;
|
||||
}
|
||||
|
||||
// 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`.
|
||||
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));
|
||||
}
|
||||
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) {
|
||||
result = processor.process_collection(
|
||||
run_config,
|
||||
history[index],
|
||||
std::span<const radar::ipc::PreprocessedCollection>(history.data(), index),
|
||||
live
|
||||
);
|
||||
}
|
||||
|
||||
if (result.collection_payloads.empty()) {
|
||||
std::printf("FAIL: sar3d produced no payloads\n");
|
||||
return 1;
|
||||
}
|
||||
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",
|
||||
payload.processing_name.c_str(), static_cast<double>(payload.scalar_value));
|
||||
}
|
||||
}
|
||||
for (const auto& payload : result.collection_payloads) {
|
||||
if (payload.kind != radar::ipc::ResultKind::ImageF32) {
|
||||
continue;
|
||||
}
|
||||
print_projection_argmax(payload, payload.processing_name == "sar3d_proj_xz" ? "Z" : "Y");
|
||||
}
|
||||
std::printf("OK: sar3d processor path completed\n");
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user