projector version
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user