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

Update GPR and SAR3D processing

Browse Source
This commit is contained in:
Ayzen
2026-10-08 16:24:01 +03:00
parent 390015ec39
commit 3575ca68f5
67 changed files with 1187 additions and 2581 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/data_processor/include/data_processor.hpp
+1 -6
View File
@@ -46,12 +46,7 @@ class DataProcessor {
// value exists, so the speed from the live config stays in force.
[[nodiscard]] auto socket_speed(const ProcessingLiveConfig& live_config) const -> std::optional<double>;
// Convert one processed collection into a locator filter spec, derived
// from the live config and current processor mode.
[[nodiscard]] auto build_locator_filter(const ProcessingLiveConfig& live_config) const
-> radar::locator::FilterParams;
void publish_locator(const ipc::ResultCollection& collection, const ProcessingLiveConfig& live_config);
void publish_locator(const ipc::ResultCollection& collection);
const config::RunConfig& config_;
config::RunConfig processing_config_;
data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp
+9 -34
View File
@@ -10,19 +10,6 @@
namespace radar::processing {
enum class LegacyGprMode {
Point,
Extended,
};
enum class LegacyGprReferenceMode {
// t_ref = midpoint between the first and last event centers.
FrameCenter,
// t_ref = center of the first event. Useful when motion offsets should be
// accumulated from frame start, e.g. for tagging frames by their head time.
FirstTxEvent,
};
struct HistoryTarget {
std::uint64_t collection_id = 0;
std::uint64_t capture_start_ns = 0;
@@ -36,15 +23,13 @@ struct ProcessingLiveConfig {
float bscan_gain = 1.0F;
float bscan_start_freq_mhz = 100.0F;
float bscan_stop_freq_mhz = 8800.0F;
LegacyGprMode legacy_gpr_mode = LegacyGprMode::Point;
// Absent in legacy live files: use the initial run_config geometry and medium.
// Absent geometry overrides use the initial run_config geometry.
std::optional<config::GprConfig> gpr{};
std::vector<std::uint32_t> gpr_input_positions{};
std::vector<std::uint32_t> gpr_output_positions{};
float gpr_min_depth_m = 2.0F;
float gpr_max_depth_m = 14.0F;
float gpr_range_comp_power = 0.1F;
float gpr_comp_power = 0.2F;
// BP object-detection stop level, as a fraction of the global peak; peaks
// below it are not objects.
float gpr_object_min_frac = 0.7F;
@@ -54,29 +39,15 @@ struct ProcessingLiveConfig {
std::string gpr_motion_mode = "int_minus";
float gpr_speed_m_s = 0.0F;
float gpr_look_angle_deg = 0.0F;
// Motion model parameters for the legacy GPR pipeline. The direction sign
// selects which way later Tx-events appear deeper (+1) or shallower (-1).
// Intra-sweep phase correction compensates the motion that happens *inside*
// one Tx-sweep before the IFFT — it is independent of the per-pair coarse
// tau shift and can be disabled without affecting the rest of the pipeline.
// The reference mode picks the anchor used to compute dt_ref per event.
float gpr_direction_sign = 1.0F;
bool gpr_apply_freq_phase_correction = true;
LegacyGprReferenceMode gpr_reference_mode = LegacyGprReferenceMode::FrameCenter;
float gpr_snr_thresh = 4.5F;
float gpr_snr_comp_max = 25.0F;
float gpr_start_freq_mhz = 3000.0F;
float gpr_stop_freq_mhz = 6000.0F;
bool gpr_background_subtract_enabled = true;
std::uint32_t gpr_background_mean_count = 10U;
bool gpr_remove_sidelobe_objects_enabled = true;
// BP image is computed in the y=imaging_plane_y_m slice of the 3D grid.
// Default 0 keeps legacy 1D antenna layouts imaging in the antenna plane.
float gpr_imaging_plane_y_m = 0.0F;
// Coherent BP object visibility (window + the draw limits below) is applied in
// the processor itself, with no score threshold. Only legacy GPR thresholds,
// on a pair count.
float legacy_gpr_min_visible_pair_count = 0.0F;
// Ground plane in radar coordinates, at this height and the configured pitch.
float gpr_bp_ground_depth_m = 1.25F;
bool pair_depth_equalize = true;
std::uint32_t gpr_max_detected_objects_to_draw = 0;
std::uint32_t gpr_draw_top_m_objects = 0;
// Cross-frame approach filter: an object is shown only once it persists as a
@@ -89,7 +60,7 @@ struct ProcessingLiveConfig {
float gpr_visible_z_min_m = 0.0F;
float gpr_visible_z_max_m = 14.0F;
// 3D SAR (sar3d processor) parameters, named after the reference notebook
// Multi_antenna_3D_clean_v1.ipynb (SAR_FRAME_RADIUS, BP_GROUND_DEPTH_M, ...).
// Multi_antenna_demonstrate_depthnorm.ipynb (SAR_FRAME_RADIUS, BP_GROUND_DEPTH_M, ...).
// The band, depth gate, antenna selection, speed and look angle are the
// shared gpr_* fields above.
std::uint32_t sar3d_frame_radius = 0U;
@@ -99,6 +70,10 @@ struct ProcessingLiveConfig {
float sar3d_comp_range_power = 0.3F;
bool sar3d_use_compensation = true;
bool sar3d_normalize_each_frame = true;
float sar3d_x_size_m = 0.0F;
float sar3d_y_min_m = -0.5F;
float sar3d_y_max_m = 2.0F;
float sar3d_z_size_m = 0.0F;
// 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.
data_acq_and_processing/processing/data_processor/src/data_processor.cpp
+19 -42
View File
@@ -22,10 +22,15 @@ namespace {
constexpr const char* kDefaultProcessorMode = "pass_through";
constexpr std::size_t kBscanReplayWindow = 50U;
[[nodiscard]] auto replay_history_limit(const config::RunConfig& config) -> std::size_t {
[[nodiscard]] auto processing_history_limit(const config::RunConfig& config, const ProcessingLiveConfig& live) -> std::size_t {
const auto preprocessed_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.preprocessed.capacity));
const auto results_capacity = static_cast<std::size_t>(std::max<std::uint32_t>(1U, config.rings.results.capacity));
return std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow});
std::size_t required = 1U;
if (live.processor_mode == "gpr" || live.processor_mode == "sar3d") {
if (live.gpr_background_subtract_enabled) { required += live.gpr_background_mean_count; }
if (live.processor_mode == "sar3d") { required += live.sar3d_frame_radius; }
}
return std::max(required, std::min({preprocessed_capacity, results_capacity, kBscanReplayWindow}));
}
void publish_result_collection(
@@ -68,7 +73,6 @@ DataProcessor::DataProcessor(
void DataProcessor::run(const std::atomic<bool>& stop_requested) {
std::vector<std::uint8_t> bytes{};
std::vector<ipc::PreprocessedCollection> preprocessed_history{};
const std::size_t history_limit = replay_history_limit(config_);
std::uint64_t last_replayed_revision = live_config_loader_.revision();
runtime::CollectionErrors errors;
std::uint64_t oversized_results = 0;
@@ -92,6 +96,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
processing_config_.gpr = live_config.gpr.value_or(config_.gpr);
}
auto& processor = resolve_processor(live_config);
const auto history_limit = processing_history_limit(config_, live_config);
const bool socket_speed_dirty = current_socket_speed != last_reprocessed_socket_speed;
const auto was_removed = [&](const ipc::PreprocessedCollection& collection) {
@@ -107,6 +112,9 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
};
if (live_revision != last_replayed_revision || socket_speed_dirty) {
std::erase_if(preprocessed_history, was_removed);
if (preprocessed_history.size() > history_limit) {
preprocessed_history.erase(preprocessed_history.begin(), preprocessed_history.end() - history_limit);
}
if (!live_config.reprocess_current_result) {
// Socket-fed speed updates should affect only future preprocessed collections,
@@ -120,7 +128,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
live_config
);
publish_result_collection(replay_result, results_ring_, oversized_results);
publish_locator(replay_result, live_config);
publish_locator(replay_result);
}
} else if (!preprocessed_history.empty()) {
const auto replay_result = process_collection(
@@ -130,7 +138,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
live_config
);
publish_result_collection(replay_result, results_ring_, oversized_results);
publish_locator(replay_result, live_config);
publish_locator(replay_result);
}
last_replayed_revision = live_revision;
//record the speed we just reprocessed with so an
@@ -156,8 +164,8 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
preprocessed.capture_start_ns <= preprocessed_history.back().capture_start_ns;
if (!fed_back) {
preprocessed_history.push_back(std::move(preprocessed));
while (preprocessed_history.size() > history_limit) {
preprocessed_history.erase(preprocessed_history.begin());
if (preprocessed_history.size() > history_limit) {
preprocessed_history.erase(preprocessed_history.begin(), preprocessed_history.end() - history_limit);
}
}
const auto& current = fed_back ? preprocessed : preprocessed_history.back();
@@ -175,7 +183,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
);
publish_result_collection(result_collection, results_ring_, oversized_results);
if (!fed_back) {
publish_locator(result_collection, live_config);
publish_locator(result_collection);
}
errors.progress();
continue;
@@ -237,40 +245,13 @@ auto DataProcessor::socket_speed(const ProcessingLiveConfig& live_config) const
return locator_server_->latest_socket_speed();
}
auto DataProcessor::build_locator_filter(const ProcessingLiveConfig& live_config) const
-> radar::locator::FilterParams {
const std::string requested_mode =
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
radar::locator::FilterParams filter{};
if (requested_mode == "legacy_gpr") {
// Legacy GPR emits its objects unfiltered, so the socket applies the legacy
// rule here: clip to the visible window and threshold on the pair count. The
// GUI disables "draw top N" for legacy, so we skip it on the wire to match.
filter.visible_bounds = radar::locator::VisibleBounds{
.x_min = live_config.gpr_visible_x_min_m,
.x_max = live_config.gpr_visible_x_max_m,
.z_min = live_config.gpr_visible_z_min_m,
.z_max = live_config.gpr_visible_z_max_m,
};
filter.min_score = live_config.legacy_gpr_min_visible_pair_count;
filter.draw_limits.reset();
}
// Coherent BP already emits the FINAL visible object set from the processor
// (window + N/M, no score threshold), so the
// socket forwards it verbatim. The default FilterParams passes everything through
// (it only drops non-finite rows), keeping the filtering logic in one place.
return filter;
}
void DataProcessor::publish_locator(
const ipc::ResultCollection& collection,
const ProcessingLiveConfig& live_config
const ipc::ResultCollection& collection
) {
if (locator_server_ == nullptr || !locator_server_->is_running()) {
if (locator_server_ == nullptr) {
return;
}
locator_server_->publish(collection, build_locator_filter(live_config));
locator_server_->publish(collection);
}
auto create_default_processors() -> ProcessorRegistry {
@@ -287,10 +268,6 @@ auto create_default_processors() -> ProcessorRegistry {
auto processor = std::make_unique<GprProcessor>();
processors.emplace(processor->name(), std::move(processor));
}
{
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));
data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp
+15 -70
View File
@@ -19,28 +19,6 @@ namespace {
using Json = nlohmann::json;
[[nodiscard]] auto parse_legacy_gpr_mode(const std::string& value, const std::string& field_name) -> LegacyGprMode {
if (value == "point") {
return LegacyGprMode::Point;
}
if (value == "extended") {
return LegacyGprMode::Extended;
}
throw std::runtime_error(field_name + " must be one of: point, extended");
}
[[nodiscard]] auto parse_legacy_gpr_reference_mode(
const std::string& value, const std::string& field_name
) -> LegacyGprReferenceMode {
if (value == "frame_center") {
return LegacyGprReferenceMode::FrameCenter;
}
if (value == "first_tx_event") {
return LegacyGprReferenceMode::FirstTxEvent;
}
throw std::runtime_error(field_name + " must be one of: frame_center, first_tx_event");
}
[[nodiscard]] auto parse_gpr_score_mode(const std::string& value, const std::string& field_name) -> std::string {
if (value == "peak" || value == "combined") {
return value;
@@ -55,24 +33,6 @@ using Json = nlohmann::json;
throw std::runtime_error(field_name + " must be one of: int_minus, int_focus");
}
void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::string& value) {
if (value == "backprojection") {
return;
}
if (value == "legacy_point") {
config.legacy_gpr_mode = LegacyGprMode::Point;
} else if (value == "legacy_extended") {
config.legacy_gpr_mode = LegacyGprMode::Extended;
} else {
throw std::runtime_error(
"processing.gpr_algorithm must be one of: backprojection, legacy_point, legacy_extended"
);
}
if (config.processor_mode.empty() || config.processor_mode == "gpr") {
config.processor_mode = "legacy_gpr";
}
}
[[nodiscard]] auto parse_f32(const Json& value, const std::string& field_name) -> float {
if (!value.is_number()) {
throw std::runtime_error(field_name + " must be number");
@@ -192,11 +152,8 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
if (const auto found = root.find("settings_revision"); found != root.end()) {
config.settings_revision = parse_u64_number(*found, "processing.settings_revision");
}
if (root.contains("relative_permittivity") || root.contains("tx_geometry") || root.contains("rx_geometry")) {
if (root.contains("tx_geometry") || root.contains("rx_geometry")) {
config.gpr = run_config.gpr;
if (const auto found = root.find("relative_permittivity"); found != root.end()) {
config.gpr->relative_permittivity = parse_f32(*found, "processing.relative_permittivity");
}
if (const auto found = root.find("tx_geometry"); found != root.end()) {
config.gpr->tx_geometry = parse_geometry(*found, "output_pos", &config::GprTxGeometry::output_pos);
}
@@ -206,25 +163,16 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
config::validate_gpr_config(*config.gpr, run_config);
}
assign_optional(root, "processor_mode", config.processor_mode);
if (!config.processor_mode.empty() && config.processor_mode != "pass_through"
&& config.processor_mode != "bscan" && config.processor_mode != "gpr" && config.processor_mode != "sar3d") {
throw std::runtime_error("Unsupported processing.processor_mode: " + config.processor_mode);
}
assign_optional(root, "bscan_axis", config.bscan_axis);
assign_optional(root, "bscan_cut_m", config.bscan_cut_m);
assign_optional(root, "bscan_max_depth_m", config.bscan_max_depth_m);
assign_optional(root, "bscan_gain", config.bscan_gain);
assign_optional(root, "bscan_start_freq_mhz", config.bscan_start_freq_mhz);
assign_optional(root, "bscan_stop_freq_mhz", config.bscan_stop_freq_mhz);
if (const auto found = root.find("legacy_gpr_mode"); found != root.end()) {
if (!found->is_string()) {
throw std::runtime_error("processing.legacy_gpr_mode must be string");
}
config.legacy_gpr_mode =
parse_legacy_gpr_mode(found->get<std::string>(), "processing.legacy_gpr_mode");
}
if (const auto found = root.find("gpr_algorithm"); found != root.end()) {
if (!found->is_string()) {
throw std::runtime_error("processing.gpr_algorithm must be string");
}
apply_legacy_gpr_algorithm_alias(config, found->get<std::string>());
}
if (const auto found = root.find("gpr_input_positions"); found != root.end()) {
config.gpr_input_positions = parse_u32_array(*found, "processing.gpr_input_positions");
}
@@ -235,7 +183,6 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
assign_optional(root, "gpr_max_depth_m", config.gpr_max_depth_m);
assign_optional(root, "gpr_range_comp_power", config.gpr_range_comp_power);
assign_optional(root, "gpr_object_min_frac", config.gpr_object_min_frac);
assign_optional(root, "gpr_comp_power", config.gpr_comp_power);
if (const auto found = root.find("gpr_score_mode"); found != root.end()) {
if (!found->is_string()) {
throw std::runtime_error("processing.gpr_score_mode must be string");
@@ -251,16 +198,6 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
assign_optional(root, "gpr_speed_m_s", config.gpr_speed_m_s);
assign_optional(root, "gpr_look_angle_deg", config.gpr_look_angle_deg);
assign_optional(root, "gpr_direction_sign", config.gpr_direction_sign);
assign_optional(root, "gpr_apply_freq_phase_correction", config.gpr_apply_freq_phase_correction);
if (const auto found = root.find("gpr_reference_mode"); found != root.end()) {
if (!found->is_string()) {
throw std::runtime_error("processing.gpr_reference_mode must be string");
}
config.gpr_reference_mode =
parse_legacy_gpr_reference_mode(found->get<std::string>(), "processing.gpr_reference_mode");
}
assign_optional(root, "gpr_snr_thresh", config.gpr_snr_thresh);
assign_optional(root, "gpr_snr_comp_max", config.gpr_snr_comp_max);
assign_optional(root, "gpr_start_freq_mhz", config.gpr_start_freq_mhz);
assign_optional(root, "gpr_stop_freq_mhz", config.gpr_stop_freq_mhz);
assign_optional(root, "gpr_background_subtract_enabled", config.gpr_background_subtract_enabled);
@@ -268,7 +205,8 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
config.gpr_background_mean_count = parse_u32_number(*found, "processing.gpr_background_mean_count");
}
assign_optional(root, "gpr_remove_sidelobe_objects_enabled", config.gpr_remove_sidelobe_objects_enabled);
assign_optional(root, "gpr_imaging_plane_y_m", config.gpr_imaging_plane_y_m);
assign_optional(root, "gpr_bp_ground_depth_m", config.gpr_bp_ground_depth_m);
assign_optional(root, "pair_depth_equalize", config.pair_depth_equalize);
for (const auto& [key, target] : {
std::pair{"gpr_visible_x_min_m", &config.gpr_visible_x_min_m},
std::pair{"gpr_visible_x_max_m", &config.gpr_visible_x_max_m},
@@ -277,7 +215,6 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
}) {
assign_optional(root, key, *target);
}
assign_optional(root, "legacy_gpr_min_visible_pair_count", config.legacy_gpr_min_visible_pair_count);
if (const auto found = root.find("gpr_max_detected_objects_to_draw"); found != root.end()) {
config.gpr_max_detected_objects_to_draw =
parse_u32_number(*found, "processing.gpr_max_detected_objects_to_draw");
@@ -297,6 +234,10 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
std::pair{"sar3d_voxel_m", &config.sar3d_voxel_m},
std::pair{"sar3d_bp_ground_depth_m", &config.sar3d_bp_ground_depth_m},
std::pair{"sar3d_comp_range_power", &config.sar3d_comp_range_power},
std::pair{"sar3d_x_size_m", &config.sar3d_x_size_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_z_size_m", &config.sar3d_z_size_m},
}) {
assign_optional(root, key, *target);
}
@@ -306,6 +247,10 @@ auto parse_geometry(const Json& value, const char* position_field, std::uint32_t
}) {
assign_optional(root, key, *target);
}
if (config.sar3d_x_size_m < 0.0F || config.sar3d_z_size_m < 0.0F ||
config.sar3d_y_max_m <= config.sar3d_y_min_m) {
throw std::runtime_error("SAR scene sizes must be non-negative and SAR_Y_RANGE_M must increase");
}
assign_optional(root, "ignore_socket_speed", config.ignore_socket_speed);
assign_optional(root, "reprocess_current_result", config.reprocess_current_result);
if (const auto found = root.find("history_clear_before_ns"); found != root.end()) {
data_acq_and_processing/processing/locator/include/locator/payload_builder.hpp
+3 -42
View File
@@ -1,7 +1,6 @@
#pragma once
#include <cstdint>
#include <optional>
#include <string>
#include <vector>
@@ -10,47 +9,9 @@
namespace radar::locator {
// Inclusive bounds used to clip locator observations to a visible window.
struct VisibleBounds {
float x_min = 0.0F;
float x_max = 0.0F;
float z_min = 0.0F;
float z_max = 0.0F;
};
// Limits mirroring the GUI semantics: if the number of detected objects exceeds
// `max_detected_objects`, the result is intentionally empty (matches Python
// reference). Otherwise, at most `draw_top_objects` rows are emitted.
struct DrawLimits {
std::uint32_t max_detected_objects = 0;
std::uint32_t draw_top_objects = 0;
};
// Filter parameters for the payload builder. None of these fields couple to GUI
// state; they are resolved by the data_processor from the live processing config
// and passed in explicitly so the builder remains a pure function.
struct FilterParams {
float min_score = 0.0F;
std::optional<VisibleBounds> visible_bounds{};
std::optional<DrawLimits> draw_limits{};
};
// Extract observations from one result collection.
//
// Mirrors `python_app/orchestration/gpr_locator.py::locator_observations_from_collection`:
// * Looks for a `gpr_points` TableF32 payload first, then falls back to
// `gpr_region_centers`. Both layouts encode `[x_m, z_m, score, ...]` rows.
// * Drops rows with non-finite coordinates and those below `min_score`.
// * If `visible_bounds` are provided, drops rows outside the inclusive window.
// * If `draw_limits` are provided and the surviving count exceeds
// `max_detected_objects`, returns an empty vector (the GUI's "too many to
// trust" heuristic). Otherwise, keeps the first `draw_top_objects` rows.
//
// The function never throws; malformed payloads degrade to an empty result.
[[nodiscard]] auto observations_from_collection(
const ipc::ResultCollection& collection,
const FilterParams& filter
) -> std::vector<Observation>;
// Extract the final coherent-GPR objects, dropping malformed or non-finite rows.
[[nodiscard]] auto observations_from_collection(const ipc::ResultCollection& collection)
-> std::vector<Observation>;
// Serialize a list of observations into the JSON payload format expected by
// locator clients: `{"ver": <n>, "tim": "HH:MM:SS.mmm", "sts": 1, "obs": [...]}`.
data_acq_and_processing/processing/locator/include/locator/tcp_server.hpp
+1 -1
View File
@@ -138,7 +138,7 @@ class TcpServer {
// Build and broadcast one locator packet derived from a GPR result
// collection. Never blocks for I/O. Filter parameters are supplied by the
// caller so the builder remains state-free.
void publish(const ipc::ResultCollection& collection, const FilterParams& filter);
void publish(const ipc::ResultCollection& collection);
// Most recent `vlc` value received from any connected client, or nullopt
// if no client has ever submitted one since startup. Lock-free.
data_acq_and_processing/processing/locator/src/payload_builder.cpp
+10 -84
View File
@@ -19,14 +19,12 @@ namespace {
using Json = nlohmann::json;
constexpr const char* kGprPointsName = "gpr_points";
constexpr const char* kGprRegionCentersName = "gpr_region_centers";
constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
// Locate the first TableF32 payload matching one of the GPR object names.
// Returns nullptr if no usable payload is present in the collection.
[[nodiscard]] auto find_object_table(const ipc::ResultCollection& collection)
-> const ipc::ResultPayload* {
const ipc::ResultPayload* fallback = nullptr;
for (const auto& payload : collection.collection_payloads) {
if (payload.kind != ipc::ResultKind::TableF32) {
continue;
@@ -37,11 +35,8 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
if (payload.processing_name == kGprPointsName) {
return &payload;
}
if (fallback == nullptr && payload.processing_name == kGprRegionCentersName) {
fallback = &payload;
}
}
return fallback;
return nullptr;
}
// Quantise to one decimal place (0.1 m = decimetres). Equivalent to Python's
@@ -51,29 +46,6 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
return std::round(value * 10.0) / 10.0;
}
[[nodiscard]] auto passes_basic_filter(
float x_m,
float z_m,
float score,
float min_score
) -> bool {
if (!std::isfinite(x_m) || !std::isfinite(z_m) || !std::isfinite(score)) {
return false;
}
return score >= min_score;
}
[[nodiscard]] auto passes_visible_bounds(
float x_m,
float z_m,
const VisibleBounds& bounds
) -> bool {
return x_m >= bounds.x_min
&& x_m <= bounds.x_max
&& z_m >= bounds.z_min
&& z_m <= bounds.z_max;
}
// Render the current wall-clock time as "HH:MM:SS.mmm". Uses localtime to match
// the Python reference behaviour (which calls datetime.now() with no tz info).
[[nodiscard]] auto format_timestamp_now() -> std::string {
@@ -107,66 +79,20 @@ void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t v
} // namespace
auto observations_from_collection(
const ipc::ResultCollection& collection,
const FilterParams& filter
) -> std::vector<Observation> {
auto observations_from_collection(const ipc::ResultCollection& collection) -> std::vector<Observation> {
const auto* payload = find_object_table(collection);
if (payload == nullptr) {
return {};
}
const std::uint32_t columns = payload->table_columns;
if (columns == 0U) {
return {};
}
const std::size_t row_count = payload->table_values.size() / columns;
if (row_count == 0U) {
return {};
}
std::vector<Observation> visible;
visible.reserve(row_count);
for (std::size_t row = 0; row < row_count; ++row) {
const std::size_t base = row * static_cast<std::size_t>(columns);
const float x_m = payload->table_values[base + 0U];
const float z_m = payload->table_values[base + 1U];
const float score = payload->table_values[base + 2U];
if (!passes_basic_filter(x_m, z_m, score, filter.min_score)) {
continue;
}
if (filter.visible_bounds.has_value()
&& !passes_visible_bounds(x_m, z_m, *filter.visible_bounds)) {
continue;
}
visible.push_back({.crs = x_m, .dst = z_m});
}
if (!filter.draw_limits.has_value()) {
std::vector<Observation> quantised;
quantised.reserve(visible.size());
for (const auto& observation : visible) {
quantised.push_back({
.crs = quantise_to_decimetres(observation.crs),
.dst = quantise_to_decimetres(observation.dst),
});
}
return quantised;
}
const auto& limits = *filter.draw_limits;
if (visible.size() > limits.max_detected_objects) {
return {};
}
const std::size_t kept = std::min<std::size_t>(visible.size(), limits.draw_top_objects);
const std::size_t columns = payload->table_columns;
std::vector<Observation> result;
result.reserve(kept);
for (std::size_t index = 0; index < kept; ++index) {
result.push_back({
.crs = quantise_to_decimetres(visible[index].crs),
.dst = quantise_to_decimetres(visible[index].dst),
});
for (std::size_t base = 0; base + columns <= payload->table_values.size(); base += columns) {
const auto x = payload->table_values[base];
const auto z = payload->table_values[base + 1];
const auto score = payload->table_values[base + 2];
if (std::isfinite(x) && std::isfinite(z) && std::isfinite(score)) {
result.push_back({.crs = quantise_to_decimetres(x), .dst = quantise_to_decimetres(z)});
}
}
return result;
}
data_acq_and_processing/processing/locator/src/tcp_server.cpp
+2 -2
View File
@@ -461,12 +461,12 @@ auto TcpServer::is_running() const -> bool {
return running_.load(std::memory_order_acquire);
}
void TcpServer::publish(const ipc::ResultCollection& collection, const FilterParams& filter) {
void TcpServer::publish(const ipc::ResultCollection& collection) {
if (!running_.load(std::memory_order_acquire)) {
return;
}
const auto observations = observations_from_collection(collection, filter);
const auto observations = observations_from_collection(collection);
const auto payload_json = build_payload_json(observations, config_.protocol_version);
auto packet = encode_packet(payload_json, config_.device_id);
cache_latest_packet(packet);
data_acq_and_processing/processing/processors/include/gpr_processor.hpp
-11
View File
@@ -21,15 +21,4 @@ class GprProcessor final : public ProcessorInterface {
ObjectApproachFilter approach_filter_{};
};
class LegacyGprProcessor final : public ProcessorInterface {
public:
[[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;
};
} // namespace radar::processing
data_acq_and_processing/processing/processors/include/sar3d_processor.hpp
+1 -1
View File
@@ -8,7 +8,7 @@
namespace radar::processing {
// 3D SAR of the reference notebook Multi_antenna_3D_clean_v1.ipynb on an NVIDIA
// 3D SAR of the reference notebook Multi_antenna_demonstrate_depthnorm.ipynb on an NVIDIA
// GPU. Publishes max-projections of the normalized volume as ImageF32 payloads:
// sar3d_front (X-Y), sar3d_side (Z-Y), sar3d_top (X-Z) and sar3d_oblique (the
// notebook's 3D scene view, axes in the plane of sar3d_oblique_basis), plus
data_acq_and_processing/processing/processors/src/backprojection.hpp
+231
View File
@@ -0,0 +1,231 @@
#pragma once
#include "shared_types.hpp"
#include <algorithm>
#include <cmath>
#include <complex>
#include <cstdint>
#include <span>
#include <map>
#include <limits>
#include <numbers>
#include <stdexcept>
#include <unordered_map>
#include <vector>
namespace radar::processing::bp {
constexpr double kWaveSpeed = 3e8; // v in both reference algorithms
using Background = std::unordered_map<ipc::ComboKey, std::vector<std::complex<double>>, ipc::ComboKeyHash>;
// Mean of up to count previous collections, separately for each physical pair.
inline auto background_mean(std::span<const ipc::PreprocessedCollection> previous, bool enabled,
std::size_t count) -> Background {
Background result;
if (!enabled || count == 0U) { return result; }
std::unordered_map<ipc::ComboKey, std::size_t, ipc::ComboKeyHash> counts;
for (const auto& collection : previous.last(std::min(count, previous.size()))) {
for (const auto& trace : collection.traces) {
const auto key = trace.combo;
auto& sum = result[key];
if (sum.empty()) { sum.resize(trace.s21.size()); }
if (sum.size() != trace.s21.size()) { continue; }
for (std::size_t i = 0; i < sum.size(); ++i) {
sum[i] += std::complex<double>(trace.s21[i].re, trace.s21[i].im);
}
++counts[key];
}
}
for (auto& [key, samples] : result) {
for (auto& sample : samples) { sample /= static_cast<double>(counts.at(key)); }
}
return result;
}
inline auto subtract_background(const ipc::SweepTraceBlock& trace, const Background& background)
-> std::vector<std::complex<double>> {
std::vector<std::complex<double>> samples;
samples.reserve(trace.s21.size());
const auto found = background.find(trace.combo);
const bool subtract = found != background.end() && found->second.size() == trace.s21.size();
for (std::size_t i = 0; i < trace.s21.size(); ++i) {
samples.emplace_back(std::complex<double>(trace.s21[i].re, trace.s21[i].im) -
(subtract ? found->second[i] : std::complex<double>{}));
}
return samples;
}
// When the samples of a trace were measured, relative to the acquisition centre
// of its collection (compute_pair_timestamps_for_snapshot and
// compute_frequency_sample_times). Traces sharing a capture window come from
// one sweep: at every frequency point its stimulus stages - the outputs, which
// are numbered in device stage order - take turns while all inputs receive.
struct TraceClock {
double start_s = 0.0; // sweep start
double event_s = 0.0; // sweep duration / (stages * points)
std::size_t stage = 0U;
std::size_t stages = 1U;
[[nodiscard]] auto sample_time_s(std::size_t point) const -> double {
return start_s + (static_cast<double>(stages) * point + stage) * event_s;
}
};
struct CollectionTiming {
std::uint64_t center_ns = 0U; // centre of the collection's capture window
std::vector<TraceClock> clocks; // per collection trace
};
// Timing from the per-trace capture windows (snapshot_acquisition_timing); every
// trace takes part, so the stages do not depend on the INPUT_IDX/OUTPUT_IDX choice.
[[nodiscard]] inline auto collection_timing(const ipc::PreprocessedCollection& collection) -> CollectionTiming {
if (collection.traces.empty()) {
throw std::runtime_error("collection has no traces");
}
std::map<std::pair<std::uint64_t, std::uint64_t>, std::vector<std::uint32_t>> sweep_outputs{};
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) {
throw std::runtime_error("collection has traces without a capture window");
}
if (trace.frequency_hz.empty()) {
throw std::runtime_error("collection has a trace without frequency points");
}
sweep_outputs[{trace.capture_start_ns, trace.capture_end_ns}].push_back(trace.combo.output_pos);
first = std::min(first, trace.capture_start_ns);
last = std::max(last, trace.capture_end_ns);
}
for (auto& [window, outputs] : sweep_outputs) {
std::sort(outputs.begin(), outputs.end());
outputs.erase(std::unique(outputs.begin(), outputs.end()), outputs.end());
}
CollectionTiming timing{};
const auto duration_ns = last - first;
timing.center_ns = first + duration_ns / 2U;
for (const auto& trace : collection.traces) {
const auto& stages = sweep_outputs.at({trace.capture_start_ns, trace.capture_end_ns});
const auto stage = std::find(stages.begin(), stages.end(), trace.combo.output_pos) - stages.begin();
TraceClock clock{};
clock.start_s = 1e-9 * (static_cast<double>(trace.capture_start_ns - first) -
0.5 * static_cast<double>(duration_ns));
clock.stage = stage;
clock.stages = stages.size();
clock.event_s = 1e-9 * static_cast<double>(trace.capture_end_ns - trace.capture_start_ns) /
static_cast<double>(clock.stages) / trace.frequency_hz.size();
timing.clocks.push_back(clock);
}
return timing;
}
// apply_intra_sweep_phase_correction: one phase model includes the pair offset
// and the intra-sweep motion. Focus mode removes its constant and linear terms.
inline void apply_motion_phase(std::vector<std::complex<double>>& samples, const std::vector<float>& frequency,
const TraceClock& clock, double speed, double pitch, double direction, bool focus,
double f_start, double f_stop) {
if (speed == 0.0) { return; }
std::vector<std::size_t> points;
std::vector<double> phase;
const double factor = 4.0 * std::numbers::pi * direction * speed * std::cos(pitch) / kWaveSpeed;
double sum_f = 0.0, sum_phase = 0.0;
for (std::size_t i = 0; i < frequency.size(); ++i) {
if (frequency[i] < f_start || frequency[i] > f_stop) { continue; }
points.push_back(i);
phase.push_back(factor * frequency[i] * clock.sample_time_s(i));
sum_f += frequency[i];
sum_phase += phase.back();
}
if (points.empty()) { return; }
if (focus) {
const double mean_f = sum_f / points.size(), mean_phase = sum_phase / points.size();
double cross = 0.0, square = 0.0;
for (std::size_t i = 0; i < points.size(); ++i) {
const double f = frequency[points[i]] - mean_f;
cross += f * phase[i]; square += f * f;
}
const double slope = square > 0.0 ? cross / square : 0.0;
for (std::size_t i = 0; i < points.size(); ++i) {
phase[i] -= mean_phase + slope * (frequency[points[i]] - mean_f);
}
}
for (std::size_t i = 0; i < points.size(); ++i) { samples[points[i]] *= std::polar(1.0, -phase[i]); }
}
// pair_depth_eq_z_max: cover the far scene corner, aperture and half a median window.
inline auto equalize_depth_limit(double max_depth, double x_min, double x_max, double antenna_x_max) -> double {
const double x_far = std::max(std::abs(x_min), std::abs(x_max)) + antenna_x_max;
return std::sqrt(max_depth * max_depth + x_far * x_far + 2.5 * 2.5) + 1.5 + 0.75;
}
// bp_plane_y, Horns_motion_4x4_ground.py: the ground plane in radar axes.
inline auto ground_y(double z, double height, double pitch) -> double {
if (std::abs(std::cos(pitch)) < 1e-6) { throw std::runtime_error("Ground BP is undefined at a look angle of 90 degrees"); }
return (-height + z * std::sin(pitch)) / std::cos(pitch);
}
// equalize_pairs_by_depth, Multi_antenna_demonstrate_depthnorm.ipynb, cell 1.
// All time axes start at zero; nearest median filtering uses the upper median
// for even windows, as scipy.ndimage.median_filter does.
template <class Real>
void equalize_pairs_by_depth(const std::vector<std::span<std::complex<Real>>>& pairs, double dz, double z_max) {
if (pairs.size() < 2U) { return; }
std::size_t length = pairs.front().size();
for (const auto pair : pairs) { length = std::min(length, pair.size()); }
const double step = std::max(1.0, std::nearbyint(0.10 / dz));
if (step > length) { return; }
const auto d = static_cast<std::size_t>(step);
const auto n = static_cast<std::size_t>(std::max(3.0, std::nearbyint(1.5 / (dz * d))));
const auto nc = static_cast<std::size_t>(std::min(static_cast<double>(length), std::ceil(z_max / dz))) / d;
if (nc < 2U) { return; }
std::vector<std::vector<double>> levels(pairs.size(), std::vector<double>(nc));
std::vector<double> common(nc, 0.0), envelope(nc), window(n);
for (std::size_t a = 0; a < pairs.size(); ++a) {
for (std::size_t i = 0; i < nc; ++i) {
double sum = 0.0;
for (std::size_t k = 0; k < d; ++k) { sum += std::abs(std::complex<double>(pairs[a][i * d + k])); }
envelope[i] = sum / static_cast<double>(d);
}
for (std::size_t i = 0; i < nc; ++i) {
for (std::size_t k = 0; k < n; ++k) {
const auto index = std::clamp<std::ptrdiff_t>(static_cast<std::ptrdiff_t>(i) +
static_cast<std::ptrdiff_t>(k) - static_cast<std::ptrdiff_t>(n / 2), 0, nc - 1);
window[k] = envelope[index];
}
std::nth_element(window.begin(), window.begin() + n / 2, window.end());
levels[a][i] = std::log(std::max(window[n / 2], 1e-30));
common[i] += levels[a][i] / static_cast<double>(pairs.size());
}
}
for (std::size_t a = 0; a < pairs.size(); ++a) {
for (std::size_t i = 0; i < nc; ++i) { levels[a][i] = std::exp(common[i] - levels[a][i]); }
for (std::size_t i = 0; i < nc * d; ++i) {
const double pos = (static_cast<double>(i) - (static_cast<double>(d) - 1.0) / 2.0) / d;
const auto j = static_cast<std::size_t>(std::clamp(std::floor(pos), 0.0, static_cast<double>(nc - 2)));
const double w = std::clamp(pos - j, 0.0, 1.0);
pairs[a][i] *= static_cast<Real>(levels[a][j] * (1.0 - w) + levels[a][j + 1] * w);
}
}
}
// Scalar median normalization follows the depth equalizer in both references.
template <class Real>
void normalize_pair(std::span<std::complex<Real>> samples, double dz, double min_depth, double max_depth) {
std::vector<double> magnitudes;
for (std::size_t i = 0; i < samples.size(); ++i) {
if (i * dz >= min_depth && i * dz <= max_depth) { magnitudes.push_back(std::abs(std::complex<double>(samples[i]))); }
}
if (magnitudes.empty()) {
for (const auto sample : samples) { magnitudes.push_back(std::abs(std::complex<double>(sample))); }
}
if (magnitudes.empty()) { return; }
const auto middle = magnitudes.begin() + magnitudes.size() / 2;
std::nth_element(magnitudes.begin(), middle, magnitudes.end());
double scale = *middle;
if (magnitudes.size() % 2 == 0) { scale = 0.5 * (scale + *std::max_element(magnitudes.begin(), middle)); }
if (!std::isfinite(scale) || scale <= 1e-15) { scale = 1.0; }
for (auto& sample : samples) { sample /= static_cast<Real>(scale + 1e-15); }
}
} // namespace radar::processing::bp
data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp
+76 -389
View File
@@ -1,5 +1,4 @@
constexpr double kPi = 3.14159265358979323846;
constexpr double kSpeedOfLightMetersPerSec = 299'792'458.0;
constexpr double kXMarginM = 2.0;
constexpr double kGridZMinM = 0.1;
constexpr std::size_t kGridWidth = 300U;
@@ -7,13 +6,8 @@ constexpr std::size_t kGridHeight = 300U;
constexpr std::size_t kAscanOversample = 8U;
constexpr double kRangeWeightMax = 5.0;
constexpr double kAngleWeightMax = 2.0;
constexpr double kTotalWeightMax = 8.0;
constexpr double kCompensationReferenceDepthM = 5.0;
constexpr bool kPairNormalize = true;
constexpr double kPairNormPercentile = 50.0;
constexpr double kPairNormEps = 1e-15;
constexpr double kSmoothSigma = 1.5;
// Gaussian-kernel half-width in sigmas, matching scipy.ndimage.gaussian_filter's
@@ -55,15 +49,8 @@ using PairKey = std::uint64_t;
// IntMinus — full intra-frequency correction to the interleaved-sweep centre.
// IntFocus — remove the constant + linear-in-frequency part of phi(f), leaving
// the focusing residual without a net Z shift.
enum class MotionMode { IntMinus, IntFocus };
[[nodiscard]] auto parse_motion_mode(const std::string& value) -> MotionMode {
return value == "int_focus" ? MotionMode::IntFocus : MotionMode::IntMinus;
}
struct GeometrySelection {
// Per-local-index Tx/Rx antenna coordinates in metres. y_/z_ default to 0
// for legacy configs so the imaging plane coincides with the antennas.
// Per-local-index Tx/Rx antenna coordinates in metres.
std::vector<std::uint32_t> input_positions{};
std::vector<std::uint32_t> output_positions{};
std::vector<double> x_tx{};
@@ -76,11 +63,6 @@ struct GeometrySelection {
std::unordered_map<std::uint32_t, std::uint32_t> output_local_by_pos{};
};
struct BackgroundAccumulator {
std::vector<std::complex<double>> sum{};
std::size_t count = 0U;
};
struct SelectedTrace {
ipc::ComboKey combo{};
std::uint32_t tx_local_index = 0U;
@@ -268,20 +250,6 @@ void fft_inplace(std::vector<std::complex<double>>& values, bool inverse) {
return axis;
}
[[nodiscard]] auto clamp_index(std::ptrdiff_t value, std::size_t limit) -> std::size_t {
if (limit == 0U) {
return 0U;
}
if (value < 0) {
return 0U;
}
const auto max_index = static_cast<std::ptrdiff_t>(limit - 1U);
if (value > max_index) {
return limit - 1U;
}
return static_cast<std::size_t>(value);
}
// scipy 'reflect' boundary (half-sample symmetric): the signal is mirrored about
// the outer edge of the first/last sample, so index -1 maps to 0, -2 to 1, n to
// n-1, and so on. Matches scipy.ndimage.gaussian_filter's default mode.
@@ -314,7 +282,7 @@ void normalize_in_place(std::vector<double>& values) {
return;
}
for (auto& value : values) {
value /= maximum;
value /= maximum + 1e-12;
}
}
@@ -545,67 +513,10 @@ void validate_collection_trace_order(
}
}
[[nodiscard]] auto build_background_mean(
std::span<const ipc::PreprocessedCollection> previous_collections,
const GeometrySelection& selection,
const ProcessingLiveConfig& live_config
) -> std::unordered_map<PairKey, std::vector<std::complex<double>>> {
std::unordered_map<PairKey, std::vector<std::complex<double>>> result{};
if (!live_config.gpr_background_subtract_enabled || live_config.gpr_background_mean_count == 0U) {
return result;
}
const std::size_t mean_count = static_cast<std::size_t>(live_config.gpr_background_mean_count);
const std::size_t start_index =
previous_collections.size() > mean_count ? previous_collections.size() - mean_count : 0U;
std::unordered_map<PairKey, BackgroundAccumulator> accumulators{};
for (std::size_t collection_index = start_index; collection_index < previous_collections.size(); ++collection_index) {
const auto& collection = previous_collections[collection_index];
for (const auto& trace : collection.traces) {
const auto output_it = selection.output_local_by_pos.find(trace.combo.output_pos);
const auto input_it = selection.input_local_by_pos.find(trace.combo.input_pos);
if (output_it == selection.output_local_by_pos.end() || input_it == selection.input_local_by_pos.end()) {
continue;
}
const auto key = make_pair_key(output_it->second, input_it->second);
auto& accumulator = accumulators[key];
if (accumulator.sum.empty()) {
accumulator.sum.assign(trace.s21.size(), std::complex<double>(0.0, 0.0));
}
if (accumulator.sum.size() != trace.s21.size()) {
continue;
}
for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) {
const auto& sample = trace.s21[sample_index];
accumulator.sum[sample_index] += std::complex<double>(sample.re, sample.im);
}
accumulator.count += 1U;
}
}
for (auto& [key, accumulator] : accumulators) {
if (accumulator.count == 0U) {
continue;
}
auto& mean_trace = result[key];
mean_trace = std::move(accumulator.sum);
const double inverse_count = 1.0 / static_cast<double>(accumulator.count);
for (auto& sample : mean_trace) {
sample *= inverse_count;
}
}
return result;
}
[[nodiscard]] auto collect_selected_traces(
const ipc::PreprocessedCollection& collection,
const GeometrySelection& selection,
const std::unordered_map<PairKey, std::vector<std::complex<double>>>& background_mean
const bp::Background& background_mean
) -> std::vector<SelectedTrace> {
std::vector<SelectedTrace> traces{};
traces.reserve(collection.traces.size());
@@ -637,15 +548,7 @@ void validate_collection_trace_order(
throw std::runtime_error("GPR requires unique selected combos; duplicate combo: " + combo_to_string(trace.combo));
}
const auto background_it = background_mean.find(key);
selected.s21.reserve(trace.s21.size());
for (std::size_t sample_index = 0U; sample_index < trace.s21.size(); ++sample_index) {
std::complex<double> sample(trace.s21[sample_index].re, trace.s21[sample_index].im);
if (background_it != background_mean.end() && background_it->second.size() == trace.s21.size()) {
sample -= background_it->second[sample_index];
}
selected.s21.push_back(sample);
}
selected.s21 = bp::subtract_background(trace, background_mean);
traces.push_back(std::move(selected));
}
@@ -653,112 +556,6 @@ void validate_collection_trace_order(
return traces;
}
// Frequency-domain speed correction applied to S21 *before* the IFFT. The measurement is interleaved Tx-by-frequency
// — Tx0(f0), Tx1(f0), Tx0(f1), ... — so each Tx occupies one interleave slot and
// its frequency k is sampled at global step (slot_count*k + slot). Each in-band
// frequency's phase is rotated back to the centre of the (full) interleaved sweep.
//
// Unlike the offline Python script, every physical input is taken from the live
// system: `speed_mps` is the socket-fed velocity, `look_angle_deg`/`direction_sign`
// are config, and `tx_sweep_time_s` is derived per-collection from the acquisition
// timestamps (capture_span / Tx count). A zero speed or non-positive sweep time
// makes this a no-op.
void apply_intra_sweep_motion_correction(
SelectedTrace& trace,
std::size_t slot,
std::size_t slot_count,
double start_hz,
double stop_hz,
double velocity_mps,
double speed_mps,
double look_angle_deg,
double direction_sign,
double tx_sweep_time_s,
MotionMode motion_mode
) {
const std::size_t n_full = trace.frequency_hz.size();
if (n_full < 2U || trace.s21.size() != n_full || slot_count == 0U
|| !(velocity_mps > 0.0) || !(tx_sweep_time_s > 0.0)) {
return;
}
const double low_hz = std::min(start_hz, stop_hz);
const double high_hz = std::max(start_hz, stop_hz);
// Absolute sample time of full-sweep frequency index k for this Tx slot.
const double dt_base = tx_sweep_time_s / static_cast<double>(n_full - 1U);
const auto sample_time_s = [&](std::size_t index) {
return ((static_cast<double>(slot_count) * static_cast<double>(index)) + static_cast<double>(slot)) * dt_base;
};
// t_center is the midpoint of the *full* interleaved sweep for this slot,
// i.e. between the first and last full-array frequencies (not the cut band).
const double t_center_s = 0.5 * (sample_time_s(0U) + sample_time_s(n_full - 1U));
const double theta_rad = (look_angle_deg * kPi) / 180.0;
const double motion_factor = direction_sign * speed_mps * std::cos(theta_rad);
std::vector<std::size_t> band_indices{};
std::vector<double> band_frequency_hz{};
std::vector<double> phi{};
band_indices.reserve(n_full);
band_frequency_hz.reserve(n_full);
phi.reserve(n_full);
bool any_nonzero = false;
for (std::size_t index = 0U; index < n_full; ++index) {
const double frequency = trace.frequency_hz[index];
if (frequency < low_hz || frequency > high_hz) {
continue;
}
const double dt_intra_s = sample_time_s(index) - t_center_s;
const double delta_range_m = motion_factor * dt_intra_s;
const double delta_path_m = 2.0 * delta_range_m;
const double dtau_s = delta_path_m / velocity_mps;
if (dtau_s != 0.0) {
any_nonzero = true;
}
band_indices.push_back(index);
band_frequency_hz.push_back(frequency);
phi.push_back(2.0 * kPi * frequency * dtau_s);
}
if (!any_nonzero || band_indices.empty()) {
return;
}
// int_focus removes the constant + linear-in-frequency component of phi(f).
// With f_rel = f - mean(f) the design columns [1, f_rel] are orthogonal, so
// the least-squares fit is intercept = mean(phi), slope = <f_rel, phi>/<f_rel, f_rel>.
if (motion_mode == MotionMode::IntFocus) {
const auto count = static_cast<double>(band_frequency_hz.size());
double mean_frequency = 0.0;
double mean_phi = 0.0;
for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
mean_frequency += band_frequency_hz[i];
mean_phi += phi[i];
}
mean_frequency /= count;
mean_phi /= count;
double cross = 0.0;
double f_rel_sq = 0.0;
for (std::size_t i = 0U; i < band_frequency_hz.size(); ++i) {
const double f_rel = band_frequency_hz[i] - mean_frequency;
cross += f_rel * phi[i];
f_rel_sq += f_rel * f_rel;
}
const double slope = (f_rel_sq > 0.0) ? (cross / f_rel_sq) : 0.0;
for (std::size_t i = 0U; i < phi.size(); ++i) {
const double f_rel = band_frequency_hz[i] - mean_frequency;
phi[i] -= mean_phi + (slope * f_rel);
}
}
for (std::size_t i = 0U; i < band_indices.size(); ++i) {
trace.s21[band_indices[i]] *= std::polar(1.0, -phi[i]);
}
}
[[nodiscard]] auto compute_ascan(
const SelectedTrace& trace,
double start_hz,
@@ -804,7 +601,7 @@ void apply_intra_sweep_motion_correction(
return result;
}
const auto start_bin = static_cast<std::int64_t>(std::llround(frequency_hz.front() / df_hz));
const auto start_bin = static_cast<std::int64_t>(std::nearbyint(frequency_hz.front() / df_hz));
if (start_bin < 0) {
return result;
}
@@ -842,50 +639,6 @@ void apply_intra_sweep_motion_correction(
return result;
}
void normalize_pair_ascans(
std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
double velocity_mps,
double min_depth_m,
double max_depth_m
) {
if (!kPairNormalize) {
return;
}
for (auto& item : ascans_by_pair) {
auto& ascan = item.second;
if (ascan.samples.empty() || ascan.time_s.size() != ascan.samples.size()) {
continue;
}
std::vector<double> amplitudes{};
amplitudes.reserve(ascan.samples.size());
for (std::size_t index = 0U; index < ascan.samples.size(); ++index) {
const double depth_m = 0.5 * velocity_mps * ascan.time_s[index];
if (depth_m < min_depth_m || depth_m > max_depth_m) {
continue;
}
amplitudes.push_back(std::abs(ascan.samples[index]));
}
if (amplitudes.empty()) {
amplitudes.reserve(ascan.samples.size());
for (const auto& sample : ascan.samples) {
amplitudes.push_back(std::abs(sample));
}
}
double scale = percentile_copy(std::move(amplitudes), kPairNormPercentile);
if (!std::isfinite(scale) || !(scale > kPairNormEps)) {
scale = 1.0;
}
const double denominator = scale + kPairNormEps;
for (auto& sample : ascan.samples) {
sample /= denominator;
}
}
}
[[nodiscard]] auto distance_3d(double dx, double dy, double dz) -> double {
return std::sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
@@ -894,7 +647,7 @@ void normalize_pair_ascans(
const GeometrySelection& selection,
double max_depth_m,
double min_z_m,
double imaging_plane_y_m
double ground_depth_m, double pitch_rad
) -> GridDefinition {
GridDefinition grid{};
if (selection.x_tx.empty() || selection.x_rx.empty() || !(max_depth_m > min_z_m)) {
@@ -922,14 +675,14 @@ void normalize_pair_ascans(
for (std::size_t tx_index = 0U; tx_index < selection.x_tx.size(); ++tx_index) {
grid.tx_distance_grids[tx_index][cell_index] = distance_3d(
x_value - selection.x_tx[tx_index],
imaging_plane_y_m - selection.y_tx[tx_index],
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_tx[tx_index],
z_value - selection.z_tx[tx_index]
);
}
for (std::size_t rx_index = 0U; rx_index < selection.x_rx.size(); ++rx_index) {
grid.rx_distance_grids[rx_index][cell_index] = distance_3d(
x_value - selection.x_rx[rx_index],
imaging_plane_y_m - selection.y_rx[rx_index],
bp::ground_y(z_value, ground_depth_m, pitch_rad) - selection.y_rx[rx_index],
z_value - selection.z_rx[rx_index]
);
}
@@ -946,10 +699,10 @@ void normalize_pair_ascans(
const GeometrySelection& selection,
double max_depth_m,
double min_z_m,
double imaging_plane_y_m
double ground_depth_m, double pitch_rad
) -> std::vector<double> {
std::vector<double> signature;
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 3U);
std::vector<double> signature{static_cast<double>(selection.x_tx.size()), static_cast<double>(selection.x_rx.size())};
signature.reserve((selection.x_tx.size() + selection.x_rx.size()) * 3U + 6U);
const auto append = [&](const std::vector<double>& axis) {
signature.insert(signature.end(), axis.begin(), axis.end());
};
@@ -961,7 +714,8 @@ void normalize_pair_ascans(
append(selection.z_rx);
signature.push_back(max_depth_m);
signature.push_back(min_z_m);
signature.push_back(imaging_plane_y_m);
signature.push_back(ground_depth_m);
signature.push_back(pitch_rad);
return signature;
}
@@ -983,67 +737,18 @@ void normalize_pair_ascans(
return (ascan.samples[index] * (1.0 - frac)) + (ascan.samples[index + 1U] * frac);
}
[[nodiscard]] auto attenuation_components(
double r_tx,
double r_rx,
double dz_tx,
double dz_rx
) -> std::pair<double, double> {
// Antennas boresight along +Z, so cos(theta) = (z_pixel - z_antenna) / R.
const double geo = 1.0 / ((r_tx * r_rx) + 1e-12);
const double angle =
std::pow(dz_tx / (r_tx + 1e-12), 2.0) *
std::pow(dz_rx / (r_rx + 1e-12), 2.0);
return {geo + 1e-30, angle + 1e-30};
}
[[nodiscard]] auto attenuation_components_at_ref_depth(
std::uint32_t tx_index,
std::uint32_t rx_index,
const GeometrySelection& selection,
double imaging_plane_y_m
) -> std::pair<double, double> {
const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
const double dz_tx = kCompensationReferenceDepthM - selection.z_tx[tx_index];
const double dz_rx = kCompensationReferenceDepthM - selection.z_rx[rx_index];
const double r_tx = distance_3d(
x_center - selection.x_tx[tx_index],
imaging_plane_y_m - selection.y_tx[tx_index],
dz_tx
);
const double r_rx = distance_3d(
x_center - selection.x_rx[rx_index],
imaging_plane_y_m - selection.y_rx[rx_index],
dz_rx
);
return attenuation_components(r_tx, r_rx, dz_tx, dz_rx);
}
[[nodiscard]] auto compensation_weight(
double r_tx,
double r_rx,
double dz_tx,
double dz_rx,
double geo_ref,
double angle_ref,
double range_power,
double angle_power
// attenuation_geo_at_ref_depth, Horns_motion_4x4_ground.py (no angle compensation).
[[nodiscard]] auto attenuation_geo_at_ref_depth(
std::uint32_t tx, std::uint32_t rx, const GeometrySelection& selection,
double ground_depth_m, double pitch_rad
) -> double {
const auto [geo, angle] = attenuation_components(r_tx, r_rx, dz_tx, dz_rx);
const double geo_norm = geo / geo_ref;
const double angle_norm = angle / angle_ref;
const double range_weight = std::clamp(
1.0 / (std::pow(geo_norm, range_power) + 1e-12),
0.0,
kRangeWeightMax
);
const double angle_weight = std::clamp(
1.0 / (std::pow(angle_norm, angle_power) + 1e-12),
0.0,
kAngleWeightMax
);
return std::clamp(range_weight * angle_weight, 0.0, kTotalWeightMax);
const double x_center = 0.5 * (selection.x_tx[tx] + selection.x_rx[rx]);
const double y = bp::ground_y(kCompensationReferenceDepthM, ground_depth_m, pitch_rad);
const double r_tx = distance_3d(x_center - selection.x_tx[tx], y - selection.y_tx[tx],
kCompensationReferenceDepthM - selection.z_tx[tx]);
const double r_rx = distance_3d(x_center - selection.x_rx[rx], y - selection.y_rx[rx],
kCompensationReferenceDepthM - selection.z_rx[rx]);
return 1.0 / (r_tx * r_rx + 1e-12) + 1e-30;
}
[[nodiscard]] auto backproject_coherent(
@@ -1051,12 +756,11 @@ void normalize_pair_ascans(
const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
const GridDefinition& grid,
const GeometrySelection& selection,
double imaging_plane_y_m,
double ground_depth_m, double pitch_rad,
double velocity_mps,
double min_depth_m,
double max_depth_m,
double range_power,
double angle_power
double range_power
) -> BpMap {
const std::size_t width = grid.x_grid.size();
const std::size_t height = grid.z_grid.size();
@@ -1076,9 +780,6 @@ void normalize_pair_ascans(
const std::vector<double>* rx_distances;
const AscanResult* ascan;
double geo_ref;
double angle_ref;
double z_tx_ant;
double z_rx_ant;
};
std::vector<PairContribution> contributions;
@@ -1089,20 +790,17 @@ void normalize_pair_ascans(
if (ascan_it == ascans_by_pair.end() || ascan_it->second.time_s.empty()) {
continue;
}
const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth(
const double geo_ref = attenuation_geo_at_ref_depth(
trace.tx_local_index,
trace.rx_local_index,
selection,
imaging_plane_y_m
ground_depth_m, pitch_rad
);
contributions.push_back(PairContribution{
&grid.tx_distance_grids[trace.tx_local_index],
&grid.rx_distance_grids[trace.rx_local_index],
&ascan_it->second,
geo_ref,
angle_ref,
selection.z_tx[trace.tx_local_index],
selection.z_rx[trace.rx_local_index],
});
}
@@ -1133,16 +831,9 @@ void normalize_pair_ascans(
}
const auto sample = interpolate_complex(ascan, tau_s);
const double weight = compensation_weight(
r_tx,
r_rx,
z_m - contribution.z_tx_ant,
z_m - contribution.z_rx_ant,
contribution.geo_ref,
contribution.angle_ref,
range_power,
angle_power
);
const double geo = 1.0 / (r_tx * r_rx + 1e-12) + 1e-30;
const double weight = std::clamp(1.0 / (std::pow(geo / contribution.geo_ref, range_power) + 1e-12),
0.0, kRangeWeightMax);
coherent_sum += sample * weight;
incoherent_sum += std::abs(sample) * weight;
contribution_count += 1.0;
@@ -1151,8 +842,8 @@ void normalize_pair_ascans(
if (!(contribution_count > 0.0)) {
continue;
}
coherent_sum /= contribution_count;
incoherent_sum /= contribution_count;
coherent_sum /= contribution_count + 1e-12;
incoherent_sum /= contribution_count + 1e-12;
result.coherent[cell_index] = coherent_sum;
result.incoherent[cell_index] = incoherent_sum;
result.image[cell_index] = std::abs(coherent_sum);
@@ -1503,7 +1194,7 @@ void apply_depth_gate(
const ObjectRecord& object,
const std::vector<SelectedTrace>& selected_traces,
const GeometrySelection& selection,
double imaging_plane_y_m
double ground_depth_m, double pitch_rad
) -> std::vector<double> {
std::vector<double> signature{};
signature.reserve(selected_traces.size());
@@ -1512,12 +1203,12 @@ void apply_depth_gate(
const auto rx = trace.rx_local_index;
const double r_tx = distance_3d(
object.x_m - selection.x_tx[tx],
imaging_plane_y_m - selection.y_tx[tx],
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_tx[tx],
object.z_m - selection.z_tx[tx]
);
const double r_rx = distance_3d(
object.x_m - selection.x_rx[rx],
imaging_plane_y_m - selection.y_rx[rx],
bp::ground_y(object.z_m, ground_depth_m, pitch_rad) - selection.y_rx[rx],
object.z_m - selection.z_rx[rx]
);
signature.push_back(0.5 * (r_tx + r_rx));
@@ -1529,12 +1220,12 @@ void mark_sidelobe_candidates(
std::vector<ObjectRecord>& objects,
const std::vector<SelectedTrace>& selected_traces,
const GeometrySelection& selection,
double imaging_plane_y_m
double ground_depth_m, double pitch_rad
) {
std::vector<std::vector<double>> signatures{};
signatures.reserve(objects.size());
for (const auto& object : objects) {
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, imaging_plane_y_m));
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, ground_depth_m, pitch_rad));
}
for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
@@ -1849,15 +1540,15 @@ void add_bp_score_metrics(
}
validate_collection_trace_order(run_config, collection);
const auto background_mean = build_background_mean(previous_collections, selection, live_config);
const auto background_mean = bp::background_mean(previous_collections, live_config.gpr_background_subtract_enabled,
live_config.gpr_background_mean_count);
auto selected_traces = collect_selected_traces(collection, selection, background_mean);
if (selected_traces.empty()) {
return results;
}
// Coherent BP fixes the medium to vacuum/air (eps_r = 1); the configurable
// relative_permittivity is a legacy-GPR-only knob.
const double velocity_mps = kSpeedOfLightMetersPerSec;
// Coherent BP propagates in air, as in Horns_motion_4x4_ground.py.
const double velocity_mps = bp::kWaveSpeed;
const double start_hz = static_cast<double>(live_config.gpr_start_freq_mhz) * 1'000'000.0;
const double stop_hz = static_cast<double>(live_config.gpr_stop_freq_mhz) * 1'000'000.0;
const double min_depth_m = static_cast<double>(live_config.gpr_min_depth_m);
@@ -1866,36 +1557,12 @@ void add_bp_score_metrics(
return results;
}
// Intra-sweep speed correction before the IFFT. The Tx interleave slot is the
// Tx local index (outputs are sorted), with slot_count = number of selected Tx.
// tx_sweep_time is derived per-collection from the acquisition timestamps:
// one Tx's interleaved sweep spans the whole frame, so capture_span / N_tx.
// Speed comes from the socket-fed live config; look-angle/direction from config.
const auto motion_mode = parse_motion_mode(live_config.gpr_motion_mode);
const std::size_t motion_slot_count = selection.output_positions.size();
const double capture_span_s =
collection.capture_end_ns > collection.capture_start_ns
? static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9
: 0.0;
const double tx_sweep_time_s =
motion_slot_count > 0U ? capture_span_s / static_cast<double>(motion_slot_count) : 0.0;
const double motion_speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
const double motion_look_angle_deg = static_cast<double>(live_config.gpr_look_angle_deg);
const double motion_direction_sign = static_cast<double>(live_config.gpr_direction_sign);
const auto timing = bp::collection_timing(collection);
const double motion_look_angle_deg = live_config.gpr_look_angle_deg;
for (auto& trace : selected_traces) {
apply_intra_sweep_motion_correction(
trace,
trace.tx_local_index,
motion_slot_count,
start_hz,
stop_hz,
velocity_mps,
motion_speed_mps,
motion_look_angle_deg,
motion_direction_sign,
tx_sweep_time_s,
motion_mode
);
bp::apply_motion_phase(trace.s21, collection.traces[trace.run_order].frequency_hz, timing.clocks[trace.run_order],
live_config.gpr_speed_m_s, motion_look_angle_deg * kPi / 180.0,
live_config.gpr_direction_sign, live_config.gpr_motion_mode == "int_focus", start_hz, stop_hz);
}
std::unordered_map<PairKey, AscanResult> ascans_by_pair{};
@@ -1912,9 +1579,30 @@ void add_bp_score_metrics(
if (ascans_by_pair.empty()) {
return results;
}
normalize_pair_ascans(ascans_by_pair, velocity_mps, min_depth_m, max_depth_m);
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
std::vector<std::span<std::complex<double>>> pairs;
for (const auto& trace : selected_traces) {
const auto found = ascans_by_pair.find(make_pair_key(trace.tx_local_index, trace.rx_local_index));
if (found != ascans_by_pair.end()) { pairs.emplace_back(found->second.samples); }
}
const double dt = ascans_by_pair.begin()->second.dt_s;
double x_min = selection.x_tx.front(), x_max = x_min, max_abs_x = 0.0;
for (const auto* xs : {&selection.x_tx, &selection.x_rx}) {
for (const double x : *xs) {
x_min = std::min(x_min, x); x_max = std::max(x_max, x); max_abs_x = std::max(max_abs_x, std::abs(x));
}
}
if (live_config.pair_depth_equalize) {
for (const auto& [key, ascan] : ascans_by_pair) {
if (ascan.dt_s != dt) { throw std::runtime_error("PAIR_DEPTH_EQUALIZE requires a common frequency grid"); }
}
bp::equalize_pairs_by_depth(pairs, dt * velocity_mps / 2.0,
bp::equalize_depth_limit(max_depth_m, x_min - kXMarginM, x_max + kXMarginM, max_abs_x));
}
for (auto& [key, ascan] : ascans_by_pair) {
bp::normalize_pair(std::span(ascan.samples), ascan.dt_s * velocity_mps / 2.0, min_depth_m, max_depth_m);
}
const double ground_depth_m = live_config.gpr_bp_ground_depth_m;
const double pitch_rad = motion_look_angle_deg * kPi / 180.0;
// The imaging grid (90k cells x 6 distance fields) only depends on antenna
// geometry and the depth window, which are constant across a run, so it is
@@ -1923,9 +1611,9 @@ void add_bp_score_metrics(
// backprojection workers join before it returns), so a local static is safe.
static std::vector<double> cached_grid_signature;
static GridDefinition cached_grid;
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
const auto signature = grid_signature(selection, max_depth_m, kGridZMinM, ground_depth_m, pitch_rad);
if (cached_grid.x_grid.empty() || signature != cached_grid_signature) {
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
cached_grid = build_grid(selection, max_depth_m, kGridZMinM, ground_depth_m, pitch_rad);
cached_grid_signature = signature;
}
const GridDefinition& grid = cached_grid;
@@ -1938,12 +1626,11 @@ void add_bp_score_metrics(
ascans_by_pair,
grid,
selection,
imaging_plane_y_m,
ground_depth_m, pitch_rad,
velocity_mps,
min_depth_m,
max_depth_m,
std::max(0.0, static_cast<double>(live_config.gpr_range_comp_power)),
0.0 // angle compensation is fixed off for coherent BP
std::max(0.0, static_cast<double>(live_config.gpr_range_comp_power))
);
if (bp.image.empty()) {
return results;
@@ -1956,7 +1643,7 @@ void add_bp_score_metrics(
auto objects = find_bp_objects(display_map, grid, static_cast<double>(live_config.gpr_object_min_frac));
add_local_prominence_metrics(objects, display_map, grid, min_depth_m, max_depth_m);
add_incoherent_support_metrics(objects, incoherent_display_map, bp.coherence_factor);
mark_sidelobe_candidates(objects, selected_traces, selection, imaging_plane_y_m);
mark_sidelobe_candidates(objects, selected_traces, selection, ground_depth_m, pitch_rad);
add_bp_score_metrics(objects, live_config);
if (live_config.gpr_remove_sidelobe_objects_enabled) {
data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp
-1066
View File
File diff suppressed because it is too large Load Diff
data_acq_and_processing/processing/processors/src/gpr_processor.cpp
+1 -14
View File
@@ -1,4 +1,5 @@
#include "gpr_processor.hpp"
#include "backprojection.hpp"
#include <algorithm>
#include <cmath>
@@ -22,7 +23,6 @@ namespace {
// Algorithm bodies stay private to this translation unit; the public processor remains a small dispatcher.
#include "gpr_backprojection_processor.ipp"
#include "gpr_legacy_processor.ipp"
} // namespace
@@ -39,17 +39,4 @@ auto GprProcessor::process_collection(
return process_backprojection_gpr(run_config, collection, previous_collections, live_config, approach_filter_);
}
auto LegacyGprProcessor::name() const -> std::string {
return "legacy_gpr";
}
auto LegacyGprProcessor::process_collection(
const config::RunConfig& run_config,
const ipc::PreprocessedCollection& collection,
std::span<const ipc::PreprocessedCollection> previous_collections,
const ProcessingLiveConfig& live_config
) -> ipc::ResultCollection {
return process_legacy_gpr(run_config, collection, previous_collections, live_config);
}
} // namespace radar::processing
data_acq_and_processing/processing/processors/src/sar3d_gpu.cpp
+36
View File
@@ -226,6 +226,34 @@ extern "C" __global__ void peak_planes(
indices[y] = index;
}
// Contiguous half-maximum interval through the joint peak on each scene axis.
// Interpolate the two crossings; use one voxel as the resolution floor.
extern "C" __global__ void peak_widths(
const float* volume, const double* gx, const double* gy, const double* gz,
const int nx, const int ny, const int nz, const int px, const int py, const int pz,
float* widths)
{
const int a = (int)(blockIdx.x * blockDim.x + threadIdx.x);
if (a >= 3) return;
const int count[3] = {nx, ny, nz}, stride[3] = {1, nx*nz, nx}, peak[3] = {px, py, pz};
const double* axis = a == 0 ? gx : a == 1 ? gy : gz;
const int origin = (py*nz + pz)*nx + px, k = peak[a], step = stride[a];
const float half = .5f * volume[origin];
int low = k, high = k;
while (low > 0 && volume[origin + (low-k-1)*step] >= half) --low;
while (high+1 < count[a] && volume[origin + (high-k+1)*step] >= half) ++high;
double left = axis[low], right = axis[high];
if (low > 0) {
const float inside = volume[origin + (low-k)*step], outside = volume[origin + (low-k-1)*step];
left -= (axis[low]-axis[low-1]) * (inside-half)/(inside-outside);
}
if (high+1 < count[a]) {
const float inside = volume[origin + (high-k)*step], outside = volume[origin + (high-k+1)*step];
right += (axis[high+1]-axis[high]) * (inside-half)/(inside-outside);
}
widths[a] = (float)fmax(right-left, axis[1]-axis[0]);
}
// Orthographic max projection. The volume is non-negative, so float bit
// patterns order like the values and an integer atomicMax does the job. The
// pixel is found in double, so it does not hinge on float rounding.
@@ -417,6 +445,7 @@ struct Gpu::Impl {
CUfunction smooth = module.function("smooth");
CUfunction max_project = module.function("max_project");
CUfunction peak_planes = module.function("peak_planes");
CUfunction peak_widths = module.function("peak_widths");
CUfunction project_oblique = module.function("project_oblique");
int smooth_radius = 0;
DeviceBuffer taps;
@@ -510,6 +539,13 @@ auto Gpu::Impl::project(const VolumeRequest& request) -> Projections {
const auto plane_levels = download<float>(levels, static_cast<std::size_t>(ny));
const auto plane_indices = download<unsigned int>(indices, static_cast<std::size_t>(ny));
choose_peak(result, request, plane_levels, plane_indices);
if (result.level > 0.0F) {
const auto widths = gpu.peak_stats.reserve(3U * sizeof(float));
launch(gpu.peak_widths, 3, volume, gx, gy, gz, nx, ny, nz,
static_cast<int>(result.peak[2]), static_cast<int>(result.peak[0]), static_cast<int>(result.peak[1]), widths);
const auto measured = download<float>(widths, 3U);
std::copy(measured.begin(), measured.end(), result.target_width.begin());
}
launch(gpu.max_project, front_size, volume, front, front_size, nx, plane, 1, nz, nx);
launch(gpu.max_project, side_size, volume, side, side_size, nz, plane, nx, nx, 1);
launch(gpu.max_project, top_size, volume, top, top_size, nx, nx, 1, ny, plane);
data_acq_and_processing/processing/processors/src/sar3d_gpu.hpp
+2 -1
View File
@@ -10,7 +10,7 @@
// CUDA backend of the sar3d processor: A-scans of the band spectra, and the
// backprojection of an aperture into the 3D volume, smoothed and projected.
// Only the projections and peak leave the backend.
// Only the projections, peak and its axial half-maximum widths leave the backend.
namespace radar::processing::sar3d {
class CudaUnavailable : public std::runtime_error {
@@ -70,6 +70,7 @@ struct Projections {
float level = 0.0F;
float raw_level = 0.0F;
std::array<std::size_t, 3> peak{}; // joint voxel maximum, [y, z, x]
std::array<float, 3> target_width{}; // contiguous FWHM through the peak, [X, Y, Z], metres
};
// CUDA context and runtime-compiled kernels. A missing device raises
data_acq_and_processing/processing/processors/src/sar3d_processor.cpp
+81 -127
View File
@@ -1,4 +1,5 @@
#include "sar3d_processor.hpp"
#include "backprojection.hpp"
#include <algorithm>
#include <array>
@@ -23,8 +24,8 @@
#include "sar3d_gpu.hpp"
// Port of the 3D SAR part of the reference notebook Multi_antenna_3D_clean_v1.ipynb
// (cells 0 and 4). Comments name the notebook functions and constants each step
// Port of the 3D SAR part of the reference notebook Multi_antenna_demonstrate_depthnorm.ipynb
// (cells 1 and 14). Comments name the notebook functions and constants each step
// follows; the parameters keep the notebook names in the GUI.
namespace radar::processing {
namespace {
@@ -32,10 +33,8 @@ namespace {
using std::numbers::pi;
// Notebook constants that are not live parameters.
constexpr double kWaveSpeed = 3e8; // v, eps_r = 1
constexpr double kWaveSpeed = bp::kWaveSpeed; // v, eps_r = 1
constexpr std::size_t kOversample = 8U; // BP_OVERSAMPLE
constexpr double kPairNormPercentile = 50.0; // PAIR_NORM_PERCENTILE
constexpr double kPairNormEps = 1e-15; // PAIR_NORM_EPS
constexpr double kCompRefDepth = 5.0; // COMP_REF_DEPTH
constexpr float kCompRangeWeightMax = 5.0F; // COMP_RANGE_WEIGHT_MAX
constexpr double kEdgeFrameWeight = 0.8; // SAR_EDGE_FRAME_WEIGHT
@@ -45,9 +44,6 @@ constexpr double kNormEdgeMargin = 0.3; // SAR_NORM_EDGE_MARGIN_M
// (x_ant.min() - 2 ... x_ant.max() + 2), Y is the height above the ground
// (SAR_Y_RANGE_M), Z along the ground follows the depth gate.
constexpr double kSceneXMargin = 2.0;
constexpr double kSceneYMin = -0.5;
constexpr double kSceneYMax = 2.0;
static_assert(kSceneYMax - kSceneYMin > 2.5 * kNormEdgeMargin, "else sar_norm_reference_mask keeps every plane");
// The 3D view looks along the camera of the notebook's 3D scene
// (sar_scene_layout: Plotly eye (1.7, -2.2, 0.85) in its axes X, Z, Y).
constexpr std::array<double, 3> kCameraEyeXzy{1.7, -2.2, 0.85};
@@ -71,10 +67,17 @@ struct Settings {
bool use_compensation = true; // SAR_USE_COMPENSATION
bool normalize_each_frame = true; // SAR_NORMALIZE_EACH_FRAME
bool pair_depth_equalize = true; // PAIR_DEPTH_EQUALIZE
bool subtract_mean = true;
std::size_t mean_count = 10U;
double x_size_m = 0.0, z_size_m = 0.0; // 0: notebook's automatic extent
double y_min_m = -0.5, y_max_m = 2.0; // SAR_Y_RANGE_M
// Everything the cached A-scans depend on.
[[nodiscard]] auto ascan_key() const {
return std::make_tuple(inputs, outputs, f_start_hz, f_stop_hz, min_depth_m, max_depth_m, speed_m_s,
look_angle_rad, normalize_each_frame);
look_angle_rad, normalize_each_frame, pair_depth_equalize, subtract_mean, mean_count,
tx, rx, x_size_m);
}
};
using AscanKey = decltype(std::declval<const Settings&>().ascan_key());
@@ -127,6 +130,16 @@ void select_antennas(
settings.comp_range_power = live.sar3d_comp_range_power;
settings.use_compensation = live.sar3d_use_compensation;
settings.normalize_each_frame = live.sar3d_normalize_each_frame;
settings.pair_depth_equalize = live.pair_depth_equalize;
settings.subtract_mean = live.gpr_background_subtract_enabled;
settings.mean_count = live.gpr_background_mean_count;
settings.x_size_m = live.sar3d_x_size_m;
settings.z_size_m = live.sar3d_z_size_m;
settings.y_min_m = live.sar3d_y_min_m;
settings.y_max_m = live.sar3d_y_max_m;
if (settings.x_size_m < 0.0 || settings.z_size_m < 0.0 || !(settings.y_max_m > settings.y_min_m)) {
throw std::runtime_error("SAR scene sizes must be non-negative and SAR_Y_RANGE_M must increase");
}
if (!(settings.f_stop_hz > settings.f_start_hz)) {
throw std::runtime_error("F_STOP must be above F_START");
}
@@ -152,73 +165,6 @@ void select_antennas(
return *lower_it + (position - static_cast<double>(lower)) * (upper - *lower_it);
}
// When the samples of a trace were measured, relative to the acquisition centre
// of its collection (compute_pair_timestamps_for_snapshot and
// compute_frequency_sample_times). Traces sharing a capture window come from
// one sweep: at every frequency point its stimulus stages - the outputs, which
// are numbered in device stage order - take turns while all inputs receive.
struct TraceClock {
double start_s = 0.0; // sweep start
double event_s = 0.0; // sweep duration / (stages * points)
std::size_t stage = 0U;
std::size_t stages = 1U;
[[nodiscard]] auto sample_time_s(std::size_t point) const -> double {
return start_s + (static_cast<double>(stages) * point + stage) * event_s;
}
};
struct CollectionTiming {
std::uint64_t center_ns = 0U; // centre of the collection's capture window
std::vector<TraceClock> clocks; // per collection trace
};
// Timing from the per-trace capture windows (snapshot_acquisition_timing); every
// trace takes part, so the stages do not depend on the INPUT_IDX/OUTPUT_IDX choice.
[[nodiscard]] auto collection_timing(const ipc::PreprocessedCollection& collection) -> CollectionTiming {
if (collection.traces.empty()) {
throw std::runtime_error("collection has no traces");
}
std::map<std::pair<std::uint64_t, std::uint64_t>, std::vector<std::uint32_t>> sweep_outputs{};
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) {
throw std::runtime_error("collection has traces without a capture window");
}
if (trace.frequency_hz.empty()) {
throw std::runtime_error("collection has a trace without frequency points");
}
sweep_outputs[{trace.capture_start_ns, trace.capture_end_ns}].push_back(trace.combo.output_pos);
first = std::min(first, trace.capture_start_ns);
last = std::max(last, trace.capture_end_ns);
}
for (auto& [window, outputs] : sweep_outputs) {
std::sort(outputs.begin(), outputs.end());
outputs.erase(std::unique(outputs.begin(), outputs.end()), outputs.end());
}
CollectionTiming timing{};
const auto duration_ns = last - first;
timing.center_ns = first + duration_ns / 2U;
for (const auto& trace : collection.traces) {
const auto& stages = sweep_outputs.at({trace.capture_start_ns, trace.capture_end_ns});
const auto stage = std::find(stages.begin(), stages.end(), trace.combo.output_pos) - stages.begin();
TraceClock clock{};
clock.start_s = 1e-9 * (static_cast<double>(trace.capture_start_ns - first) -
0.5 * static_cast<double>(duration_ns));
clock.stage = stage;
clock.stages = stages.size();
clock.event_s = 1e-9 * static_cast<double>(trace.capture_end_ns - trace.capture_start_ns) /
static_cast<double>(clock.stages) / trace.frequency_hz.size();
timing.clocks.push_back(clock);
}
return timing;
}
// Band of one trace, ready for the IFFT (apply_intra_sweep_phase_correction and
// compute_ascan_bp up to the IFFT): the radar motion removed as a phase, then
// the Hann window over the band.
struct Band {
std::size_t first_bin = 0U; // k0, the IFFT bin of the first band point
std::size_t size = 0U; // IFFT length: 2(k0 + n - 1) rounded up to a power of two, x BP_OVERSAMPLE
@@ -226,7 +172,8 @@ struct Band {
std::vector<std::complex<float>> spectrum{};
};
[[nodiscard]] auto trace_band(const ipc::SweepTraceBlock& trace, const TraceClock& clock, const Settings& settings)
[[nodiscard]] auto trace_band(const ipc::SweepTraceBlock& trace, const bp::TraceClock& clock, const Settings& settings,
const bp::Background& background)
-> Band {
const auto& frequency = trace.frequency_hz;
if (trace.s21.size() != frequency.size()) {
@@ -269,47 +216,24 @@ struct Band {
// from the collection centre at the sample time becomes the phase
// exp(-j 4 pi f offset / v): the pair and the intra-sweep corrections in one
// (MOTION_PHASE_SIGN = -1, direction +1).
const double phase_per_hz_s = -4.0 * pi * settings.speed_m_s * std::cos(settings.look_angle_rad) / kWaveSpeed;
auto samples = bp::subtract_background(trace, background);
bp::apply_motion_phase(samples, frequency, clock, settings.speed_m_s, settings.look_angle_rad,
1.0, false, settings.f_start_hz, settings.f_stop_hz);
const auto last = static_cast<double>(points.size() - 1U);
for (std::size_t index = 0U; index < points.size(); ++index) {
const std::size_t point = points[index];
const double hann = 0.5 - 0.5 * std::cos(2.0 * pi * static_cast<double>(index) / last);
const std::complex<double> sample(trace.s21[point].re, trace.s21[point].im);
const auto sample = samples[point];
if (!std::isfinite(sample.real()) || !std::isfinite(sample.imag())) {
throw std::runtime_error("S21 contains a non-finite sample");
}
band.spectrum.emplace_back(
sample * std::polar(hann, phase_per_hz_s * frequency[point] * clock.sample_time_s(point))
sample * hann
);
}
return band;
}
// SAR_NORMALIZE_EACH_FRAME: divide a channel by the PAIR_NORM_PERCENTILE of |h|
// inside the depth gate (sar_pair_ascans).
void normalize_ascan(std::span<std::complex<float>> ascan, double dt_s, const Settings& settings) {
std::vector<double> magnitudes{};
for (std::size_t index = 0U; index < ascan.size(); ++index) {
const double depth = static_cast<double>(index) * dt_s * kWaveSpeed / 2.0;
if (depth >= settings.min_depth_m && depth <= settings.max_depth_m) {
magnitudes.push_back(std::abs(std::complex<double>(ascan[index])));
}
}
if (magnitudes.empty()) {
for (const auto& sample : ascan) {
magnitudes.push_back(std::abs(std::complex<double>(sample)));
}
}
double scale = percentile(std::move(magnitudes), kPairNormPercentile);
if (!std::isfinite(scale) || scale <= kPairNormEps) {
scale = 1.0;
}
const auto divisor = static_cast<float>(scale + kPairNormEps);
for (auto& sample : ascan) {
sample /= divisor;
}
}
// A-scans of every selected Tx/Rx pair of one collection, (Tx, Rx) order.
struct Frame {
std::uint64_t center_ns = 0U;
@@ -321,9 +245,11 @@ struct Frame {
[[nodiscard]] auto compute_frame(
const ipc::PreprocessedCollection& collection,
const Settings& settings,
std::span<const ipc::PreprocessedCollection> previous,
sar3d::Gpu& backend
) -> Frame {
const auto timing = collection_timing(collection);
const auto timing = bp::collection_timing(collection);
const auto background = bp::background_mean(previous, settings.subtract_mean, settings.mean_count);
std::vector<Band> bands{};
for (const auto output : settings.outputs) {
for (const auto input : settings.inputs) {
@@ -337,7 +263,7 @@ struct Frame {
);
}
const auto& clock = timing.clocks[static_cast<std::size_t>(trace - traces.begin())];
bands.push_back(trace_band(*trace, clock, settings));
bands.push_back(trace_band(*trace, clock, settings, background));
}
}
const Band& first = bands.front();
@@ -354,10 +280,27 @@ struct Frame {
frame.samples = first.size;
frame.dt_s = 1.0 / (static_cast<double>(first.size) * first.df_hz);
frame.ascans = backend.inverse_dft(spectra, first.spectrum.size(), first.first_bin, first.size);
if (settings.normalize_each_frame) {
for (std::size_t offset = 0U; offset < frame.ascans.size(); offset += frame.samples) {
normalize_ascan(std::span(frame.ascans).subspan(offset, frame.samples), frame.dt_s, settings);
std::vector<std::span<std::complex<float>>> pairs;
for (std::size_t offset = 0; offset < frame.ascans.size(); offset += frame.samples) {
pairs.push_back(std::span(frame.ascans).subspan(offset, frame.samples));
}
const double dz = frame.dt_s * kWaveSpeed / 2.0;
if (settings.pair_depth_equalize) {
double x_min = settings.tx.front()[0], x_max = x_min, max_abs_x = 0.0;
for (const auto* antennas : {&settings.tx, &settings.rx}) {
for (const auto& antenna : *antennas) {
x_min = std::min(x_min, antenna[0]);
x_max = std::max(x_max, antenna[0]);
max_abs_x = std::max(max_abs_x, std::abs(antenna[0]));
}
}
const double half_size = settings.x_size_m > 0.0 ? settings.x_size_m / 2.0 : (x_max - x_min) / 2.0 + kSceneXMargin;
const double center = (x_min + x_max) / 2.0;
bp::equalize_pairs_by_depth(pairs, dz,
bp::equalize_depth_limit(settings.max_depth_m, center - half_size, center + half_size, max_abs_x));
}
if (settings.normalize_each_frame) {
for (auto pair : pairs) { bp::normalize_pair(pair, dz, settings.min_depth_m, settings.max_depth_m); }
}
return frame;
}
@@ -406,7 +349,7 @@ struct Grid {
}
double z_min = std::numeric_limits<double>::infinity();
double z_max = -std::numeric_limits<double>::infinity();
for (const double height : {kSceneYMin, kSceneYMax}) {
for (const double height : {settings.y_min_m, settings.y_max_m}) {
const double depth_at_zero = -(height - settings.ground_depth_m) * sin_p;
for (const double depth : {settings.min_depth_m, settings.max_depth_m}) {
const double z = (depth - depth_at_zero) / cos_p;
@@ -414,18 +357,23 @@ struct Grid {
z_max = std::max(z_max, z);
}
}
const auto resize = [](double& low, double& high, double size) {
if (size > 0.0) { const double center = (low + high) / 2.0; low = center - size / 2.0; high = center + size / 2.0; }
};
resize(x_min, x_max, settings.x_size_m);
resize(z_min, z_max, settings.z_size_m);
// Match sar_build_full_region_grid and reject oversized products before
// allocating the axes. Every downstream kernel uses signed 32-bit indices.
const auto axis_size = [&](double span) {
return checked_axis_size(std::max(std::nearbyint(span / settings.voxel_m) + 1.0, 2.0));
};
const auto nx = axis_size(x_max - x_min);
const auto ny = axis_size(kSceneYMax - kSceneYMin);
const auto ny = axis_size(settings.y_max_m - settings.y_min_m);
const auto nz = axis_size(z_max - z_min);
if (nx > static_cast<std::size_t>(std::numeric_limits<int>::max()) / ny / nz) {
throw std::runtime_error("SAR grid exceeds 32-bit kernel indexing; increase SAR_VOXEL_M");
}
return {linspace(x_min, x_max, nx), linspace(kSceneYMin, kSceneYMax, ny),
return {linspace(x_min, x_max, nx), linspace(settings.y_min_m, settings.y_max_m, ny),
linspace(z_min, z_max, nz)};
}
@@ -584,6 +532,7 @@ struct Sar3dProcessor::State {
}
struct CachedFrame {
std::vector<SweepKey> background;
std::shared_ptr<const Frame> value;
std::string error;
};
@@ -594,21 +543,28 @@ struct Sar3dProcessor::State {
std::string last_error{};
std::uint64_t repeated_errors = 0U;
auto frame(const ipc::PreprocessedCollection& collection, const Settings& settings, sar3d::Gpu& backend)
auto frame(const ipc::PreprocessedCollection& collection, const Settings& settings,
std::span<const ipc::PreprocessedCollection> previous, sar3d::Gpu& backend)
-> std::shared_ptr<const Frame> {
if (const auto found = frames.find(sweep_key(collection)); found != frames.end()) {
std::vector<SweepKey> background;
if (settings.subtract_mean) {
for (const auto& member : previous.last(std::min(previous.size(), settings.mean_count))) {
background.push_back(sweep_key(member));
}
}
if (const auto found = frames.find(sweep_key(collection)); found != frames.end() && found->second.background == background) {
if (found->second.value == nullptr) { report_error(found->second.error); }
return found->second.value;
}
std::shared_ptr<const Frame> frame{};
std::string failure;
try {
frame = std::make_shared<const Frame>(compute_frame(collection, settings, backend));
frame = std::make_shared<const Frame>(compute_frame(collection, settings, previous, backend));
} catch (const std::exception& error) {
failure = error.what();
report_error(failure);
}
frames.emplace(sweep_key(collection), CachedFrame{frame, std::move(failure)});
frames.insert_or_assign(sweep_key(collection), CachedFrame{std::move(background), frame, std::move(failure)});
return frame;
}
@@ -631,22 +587,20 @@ struct Sar3dProcessor::State {
frames.clear();
frames_key = settings.ascan_key();
}
const auto recent = previous.last(std::min(previous.size(), 2U * settings.frame_radius));
const auto recent = previous.last(std::min(previous.size(), settings.frame_radius));
std::set<SweepKey> retained{sweep_key(collection)};
for (const auto& member : recent) { retained.insert(sweep_key(member)); }
// Prune even when the newest frame is invalid, so a bad input stream
// cannot grow the failed-frame cache forever.
std::erase_if(frames, [&](const auto& entry) { return !retained.contains(entry.first); });
const auto newest = frame(collection, settings, backend);
const auto newest = frame(collection, settings, previous, backend);
if (newest == nullptr) {
return {status_payload(2.0F, last_error)};
}
// Aperture: the newest 2r+1 collections, centred r collections before
// the newest one, so the image trails the input by r frames. Collections
// that left the aperture are dropped from the cache.
// SAR_CAUSAL_ONLY: current collection and r previous ones, current scene origin.
std::vector<std::shared_ptr<const Frame>> aperture{};
for (const auto& member : recent) {
auto member_frame = frame(member, settings, backend);
for (std::size_t i = previous.size() - recent.size(); i < previous.size(); ++i) {
auto member_frame = frame(previous[i], settings, previous.first(i), backend);
const bool same_axis = member_frame != nullptr && member_frame->samples == newest->samples &&
member_frame->dt_s == newest->dt_s;
if (same_axis) {
@@ -654,9 +608,7 @@ struct Sar3dProcessor::State {
}
}
aperture.push_back(newest);
const std::size_t centre_index =
aperture.size() > settings.frame_radius ? aperture.size() - 1U - settings.frame_radius : 0U;
const Frame& centre = *aperture[centre_index];
const Frame& centre = *newest;
std::vector<double> offsets_s{};
for (const auto& member : aperture) {
@@ -761,6 +713,8 @@ struct Sar3dProcessor::State {
basis.insert(basis.end(), oblique.view.v.begin(), oblique.view.v.end());
payloads.push_back(table_payload("sar3d_oblique_basis", 3U, std::move(basis)));
if (level > 0.0F) {
payloads.push_back(table_payload("sar3d_target_width", 3U,
{images.target_width[0], images.target_width[1], images.target_width[2]}));
payloads.push_back(table_payload("sar3d_peak", 4U, {static_cast<float>(normalization), static_cast<float>(grid.x[peak_x]),
static_cast<float>(grid.y[peak_y]),
static_cast<float>(grid.z[peak_z])}));