projector version
This commit is contained in:
@@ -90,6 +90,21 @@ 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;
|
||||
bool sar3d_use_compensation = true;
|
||||
bool sar3d_normalize_pairs = true;
|
||||
// When true, the data_processor ignores socket-supplied `vlc` updates and
|
||||
// keeps using `gpr_speed_m_s` from this file. Mirrored from the GUI's
|
||||
// "ignore socket speed" checkbox.
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
#include "bscan_processor.hpp"
|
||||
#include "gpr_processor.hpp"
|
||||
#include "passthrough_processor.hpp"
|
||||
#include "sar3d_processor.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
namespace {
|
||||
@@ -280,6 +281,10 @@ auto create_default_processors() -> ProcessorRegistry {
|
||||
auto processor = std::make_unique<LegacyGprProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
{
|
||||
auto processor = std::make_unique<Sar3dProcessor>();
|
||||
processors.emplace(processor->name(), std::move(processor));
|
||||
}
|
||||
return processors;
|
||||
}
|
||||
|
||||
|
||||
@@ -386,6 +386,45 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
config.gpr_object_approach_min_frames =
|
||||
parse_u32_number(*found, "processing.gpr_object_approach_min_frames");
|
||||
}
|
||||
if (const auto found = root.find("sar3d_frame_radius"); found != root.end()) {
|
||||
config.sar3d_frame_radius = parse_u32_number(*found, "processing.sar3d_frame_radius");
|
||||
}
|
||||
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},
|
||||
}) {
|
||||
if (const auto found = root.find(key); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error(std::string("processing.") + key + " must be number");
|
||||
}
|
||||
*target = static_cast<float>(found->get<double>());
|
||||
}
|
||||
}
|
||||
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},
|
||||
}) {
|
||||
if (const auto found = root.find(key); found != root.end()) {
|
||||
*target = parse_u32_number(*found, std::string("processing.") + key);
|
||||
}
|
||||
}
|
||||
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");
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <string>
|
||||
|
||||
#include "processor_interface.hpp"
|
||||
|
||||
namespace radar::processing {
|
||||
|
||||
// 3D SAR over a synthesized aperture, computed by the external gpr_cpp
|
||||
// CUDA/NVRTC library (VNA_AlGo). Emits 2D max-projections of the volume
|
||||
// (side XZ and top XY views) as ImageF32 collection payloads.
|
||||
class Sar3dProcessor final : public ProcessorInterface {
|
||||
public:
|
||||
Sar3dProcessor();
|
||||
~Sar3dProcessor() override;
|
||||
|
||||
[[nodiscard]] auto name() const -> std::string override;
|
||||
[[nodiscard]] auto process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection override;
|
||||
|
||||
private:
|
||||
// Pimpl keeps gpr/CUDA headers (cuda.h, nvrtc.h) out of the shared
|
||||
// processor include path; only sar3d_processor.cpp needs them.
|
||||
struct State;
|
||||
std::unique_ptr<State> state_;
|
||||
};
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -0,0 +1,521 @@
|
||||
#include "sar3d_processor.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <bit>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
#include "gpr/ascan.hpp"
|
||||
#include "gpr/config.hpp"
|
||||
#include "gpr/geometry.hpp"
|
||||
#include "gpr/sar3d.hpp"
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void hash_mix(std::size_t& seed, double value) {
|
||||
hash_mix(seed, std::bit_cast<std::uint64_t>(value));
|
||||
}
|
||||
|
||||
[[nodiscard]] auto contains_u32(const std::vector<std::uint32_t>& values, std::uint32_t value) -> bool {
|
||||
return std::find(values.begin(), values.end(), value) != values.end();
|
||||
}
|
||||
|
||||
// Requested positions filtered to the available set; empty request selects all.
|
||||
// Mirrors the coherent-BP processor's selection semantics.
|
||||
[[nodiscard]] auto selected_positions(
|
||||
const std::vector<std::uint32_t>& requested,
|
||||
std::vector<std::uint32_t> available
|
||||
) -> std::vector<std::uint32_t> {
|
||||
std::sort(available.begin(), available.end());
|
||||
available.erase(std::unique(available.begin(), available.end()), available.end());
|
||||
if (requested.empty()) {
|
||||
return available;
|
||||
}
|
||||
std::vector<std::uint32_t> result{};
|
||||
for (const auto value : requested) {
|
||||
if (contains_u32(available, value) && !contains_u32(result, value)) {
|
||||
result.push_back(value);
|
||||
}
|
||||
}
|
||||
std::sort(result.begin(), result.end());
|
||||
return result;
|
||||
}
|
||||
|
||||
struct Selection {
|
||||
// Sorted ascending switch positions; the logical pair key (i_tx, i_rx) is
|
||||
// the index of a position within these lists.
|
||||
std::vector<std::uint32_t> inputs{};
|
||||
std::vector<std::uint32_t> outputs{};
|
||||
};
|
||||
|
||||
[[nodiscard]] auto build_selection(
|
||||
const config::RunConfig& run_config,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> Selection {
|
||||
std::vector<std::uint32_t> available_inputs{};
|
||||
available_inputs.reserve(run_config.gpr.rx_geometry.size());
|
||||
for (const auto& entry : run_config.gpr.rx_geometry) {
|
||||
available_inputs.push_back(entry.input_pos);
|
||||
}
|
||||
std::vector<std::uint32_t> available_outputs{};
|
||||
available_outputs.reserve(run_config.gpr.tx_geometry.size());
|
||||
for (const auto& entry : run_config.gpr.tx_geometry) {
|
||||
available_outputs.push_back(entry.output_pos);
|
||||
}
|
||||
Selection selection{};
|
||||
selection.inputs = selected_positions(live_config.gpr_input_positions, std::move(available_inputs));
|
||||
selection.outputs = selected_positions(live_config.gpr_output_positions, std::move(available_outputs));
|
||||
return selection;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_geometry(
|
||||
const config::RunConfig& run_config,
|
||||
const Selection& selection
|
||||
) -> gpr::GeometryConfig {
|
||||
gpr::GeometryConfig geom{};
|
||||
geom.input_idx.clear();
|
||||
geom.output_idx.clear();
|
||||
geom.rx_positions.clear();
|
||||
geom.tx_positions.clear();
|
||||
for (const auto position : selection.inputs) {
|
||||
for (const auto& entry : run_config.gpr.rx_geometry) {
|
||||
if (entry.input_pos == position) {
|
||||
geom.input_idx.push_back(static_cast<int>(position));
|
||||
geom.rx_positions.push_back({
|
||||
static_cast<double>(entry.x_m),
|
||||
static_cast<double>(entry.y_m),
|
||||
static_cast<double>(entry.z_m),
|
||||
});
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const auto position : selection.outputs) {
|
||||
for (const auto& entry : run_config.gpr.tx_geometry) {
|
||||
if (entry.output_pos == position) {
|
||||
geom.output_idx.push_back(static_cast<int>(position));
|
||||
geom.tx_positions.push_back({
|
||||
static_cast<double>(entry.x_m),
|
||||
static_cast<double>(entry.y_m),
|
||||
static_cast<double>(entry.z_m),
|
||||
});
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
geom.eps_r = static_cast<double>(run_config.gpr.relative_permittivity);
|
||||
geom.finalize();
|
||||
return geom;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_bp_config(const ProcessingLiveConfig& live_config) -> gpr::BPConfig {
|
||||
gpr::BPConfig bp{};
|
||||
bp.f_start = static_cast<double>(live_config.gpr_start_freq_mhz) * 1e6;
|
||||
bp.f_stop = static_cast<double>(live_config.gpr_stop_freq_mhz) * 1e6;
|
||||
bp.min_depth = static_cast<double>(live_config.gpr_min_depth_m);
|
||||
bp.max_depth = static_cast<double>(live_config.gpr_max_depth_m);
|
||||
bp.pair_normalize = live_config.sar3d_normalize_pairs;
|
||||
bp.pair_norm_depth_min = bp.min_depth;
|
||||
bp.pair_norm_depth_max = bp.max_depth;
|
||||
return bp;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_motion_config(const ProcessingLiveConfig& live_config) -> gpr::MotionConfig {
|
||||
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);
|
||||
motion.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
|
||||
motion.apply_freq_phase_correction = live_config.gpr_apply_freq_phase_correction;
|
||||
return motion;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_sar_config(const ProcessingLiveConfig& live_config) -> gpr::SARConfig {
|
||||
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.use_compensation = live_config.sar3d_use_compensation;
|
||||
return sar;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto ascan_fingerprint(
|
||||
const Selection& selection,
|
||||
const gpr::GeometryConfig& geom,
|
||||
const gpr::BPConfig& bp,
|
||||
const gpr::MotionConfig& motion
|
||||
) -> std::size_t {
|
||||
std::size_t seed = 0U;
|
||||
for (const auto position : selection.inputs) {
|
||||
hash_mix(seed, static_cast<std::uint64_t>(position));
|
||||
}
|
||||
for (const auto position : selection.outputs) {
|
||||
hash_mix(seed, static_cast<std::uint64_t>(static_cast<std::uint64_t>(position) | (1ULL << 40U)));
|
||||
}
|
||||
hash_mix(seed, geom.eps_r);
|
||||
hash_mix(seed, bp.f_start);
|
||||
hash_mix(seed, bp.f_stop);
|
||||
hash_mix(seed, bp.pair_norm_depth_min);
|
||||
hash_mix(seed, bp.pair_norm_depth_max);
|
||||
hash_mix(seed, static_cast<std::uint64_t>(bp.pair_normalize ? 1U : 0U));
|
||||
hash_mix(seed, motion.speed_m_s);
|
||||
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));
|
||||
return seed;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto engine_fingerprint(
|
||||
const gpr::GeometryConfig& geom,
|
||||
const gpr::BPConfig& bp,
|
||||
const gpr::MotionConfig& motion,
|
||||
const gpr::SARConfig& sar
|
||||
) -> std::size_t {
|
||||
std::size_t seed = 1U;
|
||||
for (std::size_t index = 0U; index < geom.tx_positions.size(); ++index) {
|
||||
hash_mix(seed, static_cast<std::uint64_t>(geom.output_idx[index]));
|
||||
hash_mix(seed, geom.tx_positions[index][0]);
|
||||
hash_mix(seed, geom.tx_positions[index][1]);
|
||||
hash_mix(seed, geom.tx_positions[index][2]);
|
||||
}
|
||||
for (std::size_t index = 0U; index < geom.rx_positions.size(); ++index) {
|
||||
hash_mix(seed, static_cast<std::uint64_t>(geom.input_idx[index]));
|
||||
hash_mix(seed, geom.rx_positions[index][0]);
|
||||
hash_mix(seed, geom.rx_positions[index][1]);
|
||||
hash_mix(seed, geom.rx_positions[index][2]);
|
||||
}
|
||||
hash_mix(seed, geom.eps_r);
|
||||
hash_mix(seed, bp.f_start);
|
||||
hash_mix(seed, bp.f_stop);
|
||||
hash_mix(seed, bp.min_depth);
|
||||
hash_mix(seed, bp.max_depth);
|
||||
hash_mix(seed, bp.comp_range_power);
|
||||
hash_mix(seed, bp.comp_weight_max);
|
||||
hash_mix(seed, bp.comp_ref_depth);
|
||||
hash_mix(seed, motion.pitch_deg);
|
||||
hash_mix(seed, static_cast<std::uint64_t>(sar.use_compensation ? 1U : 0U));
|
||||
return seed;
|
||||
}
|
||||
|
||||
// Midpoint of the collection's capture window; monotonic_ns is the fallback for
|
||||
// producers that never stamped capture times.
|
||||
[[nodiscard]] auto collection_time_s(const ipc::PreprocessedCollection& collection) -> double {
|
||||
if (collection.capture_end_ns > collection.capture_start_ns) {
|
||||
return 0.5 * (static_cast<double>(collection.capture_start_ns) + static_cast<double>(collection.capture_end_ns)) * 1e-9;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto build_image_payload(
|
||||
const std::string& processing_name,
|
||||
const std::vector<double>& x_axis,
|
||||
const std::vector<double>& y_axis,
|
||||
std::vector<float> values
|
||||
) -> ipc::ResultPayload {
|
||||
ipc::ResultPayload payload{};
|
||||
payload.processing_name = processing_name;
|
||||
payload.kind = ipc::ResultKind::ImageF32;
|
||||
payload.image_x_axis.reserve(x_axis.size());
|
||||
for (const auto value : x_axis) {
|
||||
payload.image_x_axis.push_back(static_cast<float>(value));
|
||||
}
|
||||
payload.image_y_axis.reserve(y_axis.size());
|
||||
for (const auto value : y_axis) {
|
||||
payload.image_y_axis.push_back(static_cast<float>(value));
|
||||
}
|
||||
payload.image_values = std::move(values);
|
||||
return payload;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
struct Sar3dProcessor::State {
|
||||
struct CacheEntry {
|
||||
double time_s = 0.0;
|
||||
// Null when the collection could not be preprocessed (e.g. missing
|
||||
// pairs); cached so a bad collection is not retried every frame.
|
||||
std::shared_ptr<gpr::AScanSet> ascans{};
|
||||
};
|
||||
|
||||
// Keyed by collection_id (monotonically increasing), so evicting from the
|
||||
// front drops the oldest collections.
|
||||
std::map<std::uint64_t, CacheEntry> ascan_cache{};
|
||||
std::size_t ascan_fp = 0U;
|
||||
|
||||
std::unique_ptr<gpr::SarEngine> engine{};
|
||||
std::size_t engine_fp = 0U;
|
||||
|
||||
std::string last_error{};
|
||||
std::uint64_t suppressed_errors = 0U;
|
||||
|
||||
auto get_or_compute_ascans(
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
const Selection& selection,
|
||||
const gpr::GeometryConfig& geom,
|
||||
const gpr::BPConfig& bp,
|
||||
gpr::MotionConfig motion
|
||||
) -> 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);
|
||||
|
||||
std::map<gpr::PairKey, std::vector<gpr::c128>> s21{};
|
||||
std::map<gpr::PairKey, std::vector<double>> freq{};
|
||||
for (const auto& trace : collection.traces) {
|
||||
const auto input_it = std::find(selection.inputs.begin(), selection.inputs.end(), trace.combo.input_pos);
|
||||
const auto output_it = std::find(selection.outputs.begin(), selection.outputs.end(), trace.combo.output_pos);
|
||||
if (input_it == selection.inputs.end() || output_it == selection.outputs.end()) {
|
||||
continue;
|
||||
}
|
||||
const gpr::PairKey key{
|
||||
static_cast<int>(output_it - selection.outputs.begin()),
|
||||
static_cast<int>(input_it - selection.inputs.begin()),
|
||||
};
|
||||
auto& samples = s21[key];
|
||||
samples.clear();
|
||||
samples.reserve(trace.s21.size());
|
||||
for (const auto& sample : trace.s21) {
|
||||
samples.emplace_back(static_cast<double>(sample.re), static_cast<double>(sample.im));
|
||||
}
|
||||
auto& axis = freq[key];
|
||||
axis.assign(trace.frequency_hz.begin(), trace.frequency_hz.end());
|
||||
}
|
||||
|
||||
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);
|
||||
try {
|
||||
entry.ascans = std::make_shared<gpr::AScanSet>(gpr::preprocess_snapshot(
|
||||
s21, freq, /*background=*/nullptr, motion, geom, bp, timestamps, bp.pair_normalize
|
||||
));
|
||||
} catch (const std::exception& exc) {
|
||||
// A malformed collection (e.g. empty band after a sweep change)
|
||||
// is cached as a skipped frame instead of failing the aperture.
|
||||
report_error(std::string{"preprocess_snapshot: "} + exc.what());
|
||||
entry.ascans = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
auto [inserted, _] = ascan_cache.emplace(collection.collection_id, std::move(entry));
|
||||
while (ascan_cache.size() > kAscanCacheLimit) {
|
||||
ascan_cache.erase(ascan_cache.begin());
|
||||
}
|
||||
return inserted->second;
|
||||
}
|
||||
|
||||
void report_error(const std::string& message) {
|
||||
if (message == last_error) {
|
||||
++suppressed_errors;
|
||||
return;
|
||||
}
|
||||
if (suppressed_errors > 0U) {
|
||||
std::cerr << "sar3d: previous error repeated " << suppressed_errors << " times\n";
|
||||
}
|
||||
std::cerr << "sar3d: " << message << '\n';
|
||||
last_error = message;
|
||||
suppressed_errors = 0U;
|
||||
}
|
||||
};
|
||||
|
||||
Sar3dProcessor::Sar3dProcessor() : state_(std::make_unique<State>()) {}
|
||||
|
||||
Sar3dProcessor::~Sar3dProcessor() = default;
|
||||
|
||||
auto Sar3dProcessor::name() const -> std::string {
|
||||
return "sar3d";
|
||||
}
|
||||
|
||||
auto Sar3dProcessor::process_collection(
|
||||
const config::RunConfig& run_config,
|
||||
const ipc::PreprocessedCollection& collection,
|
||||
std::span<const ipc::PreprocessedCollection> previous_collections,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
ipc::ResultCollection results{};
|
||||
results.collection_id = collection.collection_id;
|
||||
results.monotonic_ns = collection.monotonic_ns;
|
||||
|
||||
try {
|
||||
const auto selection = build_selection(run_config, live_config);
|
||||
if (selection.inputs.empty() || selection.outputs.empty()) {
|
||||
return results;
|
||||
}
|
||||
const auto geom = build_geometry(run_config, selection);
|
||||
const auto bp = build_bp_config(live_config);
|
||||
const auto motion = build_motion_config(live_config);
|
||||
const auto sar = build_sar_config(live_config);
|
||||
if (!(bp.max_depth > bp.min_depth) || !(bp.f_stop > bp.f_start)) {
|
||||
return results;
|
||||
}
|
||||
|
||||
const auto ascan_fp = ascan_fingerprint(selection, geom, bp, motion);
|
||||
if (ascan_fp != state_->ascan_fp) {
|
||||
state_->ascan_cache.clear();
|
||||
state_->ascan_fp = ascan_fp;
|
||||
}
|
||||
|
||||
// Aperture window: the newest 2r+1 collections, current one last.
|
||||
const std::size_t window_size = 2U * static_cast<std::size_t>(sar.frame_radius) + 1U;
|
||||
std::vector<const ipc::PreprocessedCollection*> window{};
|
||||
window.reserve(window_size);
|
||||
const std::size_t history_take = std::min(previous_collections.size(), window_size - 1U);
|
||||
for (std::size_t index = previous_collections.size() - history_take; index < previous_collections.size(); ++index) {
|
||||
window.push_back(&previous_collections[index]);
|
||||
}
|
||||
window.push_back(&collection);
|
||||
|
||||
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);
|
||||
if (entry.ascans == nullptr) {
|
||||
continue;
|
||||
}
|
||||
ascan_sets.push_back(entry.ascans);
|
||||
frame_times.push_back(entry.time_s);
|
||||
}
|
||||
if (ascan_sets.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
// aperture_volume requires one shared time axis; drop frames whose axis
|
||||
// differs from the current collection's (e.g. after a sweep-grid change).
|
||||
const auto& reference = *ascan_sets.back();
|
||||
for (std::size_t index = ascan_sets.size(); index-- > 0U;) {
|
||||
const auto& candidate = *ascan_sets[index];
|
||||
if (candidate.n_fft != reference.n_fft || std::abs(candidate.t0 - reference.t0) > 1e-18 ||
|
||||
candidate.n_pairs != reference.n_pairs) {
|
||||
ascan_sets.erase(ascan_sets.begin() + static_cast<std::ptrdiff_t>(index));
|
||||
frame_times.erase(frame_times.begin() + static_cast<std::ptrdiff_t>(index));
|
||||
}
|
||||
}
|
||||
if (ascan_sets.empty()) {
|
||||
return results;
|
||||
}
|
||||
|
||||
// Symmetric aperture: the reference frame sits `radius` frames before the
|
||||
// newest one, so the published volume lags the input by `radius` frames.
|
||||
const std::size_t center_index =
|
||||
ascan_sets.size() > static_cast<std::size_t>(sar.frame_radius)
|
||||
? ascan_sets.size() - 1U - static_cast<std::size_t>(sar.frame_radius)
|
||||
: 0U;
|
||||
const auto offsets = gpr::frame_offsets(frame_times, frame_times[center_index], motion);
|
||||
const auto weights = gpr::frame_weights(offsets, sar.edge_frame_weight);
|
||||
std::vector<gpr::ApertureFrame> frames{};
|
||||
frames.reserve(ascan_sets.size());
|
||||
for (std::size_t index = 0U; index < ascan_sets.size(); ++index) {
|
||||
frames.push_back(gpr::ApertureFrame{ascan_sets[index].get(), offsets[index], weights[index]});
|
||||
}
|
||||
|
||||
const auto engine_fp = engine_fingerprint(geom, bp, motion, sar);
|
||||
if (state_->engine == nullptr || state_->engine_fp != engine_fp) {
|
||||
state_->engine = std::make_unique<gpr::SarEngine>(geom, bp, motion, sar);
|
||||
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);
|
||||
|
||||
// Finalization (port of gpr_core sar_finish_volume, minus smoothing):
|
||||
// |acc/(cnt+eps)|, ground-coordinate depth gate, normalization to peak.
|
||||
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);
|
||||
|
||||
std::vector<float> projection_xz(static_cast<std::size_t>(nz) * nx, 0.0F);
|
||||
std::vector<float> projection_xy(static_cast<std::size_t>(ny) * nx, 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;
|
||||
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);
|
||||
auto& xz_cell = projection_xz[static_cast<std::size_t>(iz) * nx + ix];
|
||||
xz_cell = std::max(xz_cell, value);
|
||||
auto& xy_cell = projection_xy[static_cast<std::size_t>(iy) * nx + ix];
|
||||
xy_cell = std::max(xy_cell, value);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (peak > 0.0F) {
|
||||
for (auto& value : projection_xz) {
|
||||
value /= peak;
|
||||
}
|
||||
for (auto& value : projection_xy) {
|
||||
value /= peak;
|
||||
}
|
||||
}
|
||||
|
||||
results.collection_payloads.push_back(
|
||||
build_image_payload("sar3d_proj_xz", grid.x, grid.z, std::move(projection_xz))
|
||||
);
|
||||
results.collection_payloads.push_back(
|
||||
build_image_payload("sar3d_proj_xy", grid.x, grid.y, std::move(projection_xy))
|
||||
);
|
||||
ipc::ResultPayload peak_payload{};
|
||||
peak_payload.processing_name = "sar3d_peak";
|
||||
peak_payload.kind = ipc::ResultKind::ScalarF32;
|
||||
peak_payload.scalar_value = peak;
|
||||
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:
|
||||
// publish an empty result and keep going. Repeated identical errors are
|
||||
// logged once with a suppression count.
|
||||
state_->report_error(exc.what());
|
||||
results.collection_payloads.clear();
|
||||
}
|
||||
|
||||
return results;
|
||||
}
|
||||
|
||||
} // namespace radar::processing
|
||||
@@ -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