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

working version

Browse Source
This commit is contained in:
Ayzen
2026-05-18 17:27:00 +03:00
parent 907dbf29ce
commit 0da8b1283c
23 changed files with 1479 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/processors/src/gpr_backprojection_processor.ipp
+126 -53
View File
@@ -49,10 +49,16 @@ constexpr double kScoreCfEps = 1e-12;
using PairKey = std::uint64_t;
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.
std::vector<std::uint32_t> input_positions{};
std::vector<std::uint32_t> output_positions{};
std::vector<double> x_tx{};
std::vector<double> y_tx{};
std::vector<double> z_tx{};
std::vector<double> x_rx{};
std::vector<double> y_rx{};
std::vector<double> z_rx{};
std::unordered_map<std::uint32_t, std::uint32_t> input_local_by_pos{};
std::unordered_map<std::uint32_t, std::uint32_t> output_local_by_pos{};
};
@@ -365,29 +371,43 @@ void normalize_in_place(std::vector<double>& values) {
return result;
}
struct AntennaXYZ {
double x_m = 0.0;
double y_m = 0.0;
double z_m = 0.0;
};
[[nodiscard]] auto build_geometry_selection(
const config::RunConfig& run_config,
const ProcessingLiveConfig& live_config
) -> GeometrySelection {
std::unordered_map<std::uint32_t, double> tx_x_by_pos{};
std::unordered_map<std::uint32_t, AntennaXYZ> tx_by_pos{};
for (const auto& entry : run_config.gpr.tx_geometry) {
tx_x_by_pos[entry.output_pos] = static_cast<double>(entry.x_m);
tx_by_pos[entry.output_pos] = AntennaXYZ{
static_cast<double>(entry.x_m),
static_cast<double>(entry.y_m),
static_cast<double>(entry.z_m),
};
}
std::unordered_map<std::uint32_t, double> rx_x_by_pos{};
std::unordered_map<std::uint32_t, AntennaXYZ> rx_by_pos{};
for (const auto& entry : run_config.gpr.rx_geometry) {
rx_x_by_pos[entry.input_pos] = static_cast<double>(entry.x_m);
rx_by_pos[entry.input_pos] = AntennaXYZ{
static_cast<double>(entry.x_m),
static_cast<double>(entry.y_m),
static_cast<double>(entry.z_m),
};
}
std::vector<std::uint32_t> available_outputs{};
available_outputs.reserve(tx_x_by_pos.size());
for (const auto& [position, _] : tx_x_by_pos) {
available_outputs.reserve(tx_by_pos.size());
for (const auto& [position, _] : tx_by_pos) {
available_outputs.push_back(position);
}
std::vector<std::uint32_t> available_inputs{};
available_inputs.reserve(rx_x_by_pos.size());
for (const auto& [position, _] : rx_x_by_pos) {
available_inputs.reserve(rx_by_pos.size());
for (const auto& [position, _] : rx_by_pos) {
available_inputs.push_back(position);
}
@@ -395,18 +415,32 @@ void normalize_in_place(std::vector<double>& values) {
selection.output_positions = selected_positions(live_config.gpr_output_positions, std::move(available_outputs));
selection.input_positions = selected_positions(live_config.gpr_input_positions, std::move(available_inputs));
selection.x_tx.reserve(selection.output_positions.size());
const auto reserve_axes = [](GeometrySelection& target, std::size_t tx_count, std::size_t rx_count) {
target.x_tx.reserve(tx_count);
target.y_tx.reserve(tx_count);
target.z_tx.reserve(tx_count);
target.x_rx.reserve(rx_count);
target.y_rx.reserve(rx_count);
target.z_rx.reserve(rx_count);
};
reserve_axes(selection, selection.output_positions.size(), selection.input_positions.size());
for (std::size_t index = 0U; index < selection.output_positions.size(); ++index) {
const auto position = selection.output_positions[index];
selection.output_local_by_pos[position] = static_cast<std::uint32_t>(index);
selection.x_tx.push_back(tx_x_by_pos[position]);
const auto& xyz = tx_by_pos[position];
selection.x_tx.push_back(xyz.x_m);
selection.y_tx.push_back(xyz.y_m);
selection.z_tx.push_back(xyz.z_m);
}
selection.x_rx.reserve(selection.input_positions.size());
for (std::size_t index = 0U; index < selection.input_positions.size(); ++index) {
const auto position = selection.input_positions[index];
selection.input_local_by_pos[position] = static_cast<std::uint32_t>(index);
selection.x_rx.push_back(rx_x_by_pos[position]);
const auto& xyz = rx_by_pos[position];
selection.x_rx.push_back(xyz.x_m);
selection.y_rx.push_back(xyz.y_m);
selection.z_rx.push_back(xyz.z_m);
}
return selection;
@@ -673,19 +707,23 @@ void normalize_pair_ascans(
}
}
[[nodiscard]] auto distance_3d(double dx, double dy, double dz) -> double {
return std::sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
[[nodiscard]] auto build_grid(
const std::vector<double>& x_tx,
const std::vector<double>& x_rx,
const GeometrySelection& selection,
double max_depth_m,
double min_z_m
double min_z_m,
double imaging_plane_y_m
) -> GridDefinition {
GridDefinition grid{};
if (x_tx.empty() || x_rx.empty() || !(max_depth_m > min_z_m)) {
if (selection.x_tx.empty() || selection.x_rx.empty() || !(max_depth_m > min_z_m)) {
return grid;
}
const auto [tx_min_it, tx_max_it] = std::minmax_element(x_tx.begin(), x_tx.end());
const auto [rx_min_it, rx_max_it] = std::minmax_element(x_rx.begin(), x_rx.end());
const auto [tx_min_it, tx_max_it] = std::minmax_element(selection.x_tx.begin(), selection.x_tx.end());
const auto [rx_min_it, rx_max_it] = std::minmax_element(selection.x_rx.begin(), selection.x_rx.end());
const double x_min = std::min(*tx_min_it, *rx_min_it) - kXMarginM;
const double x_max = std::max(*tx_max_it, *rx_max_it) + kXMarginM;
@@ -693,8 +731,8 @@ void normalize_pair_ascans(
grid.z_grid = build_axis(min_z_m, max_depth_m, kGridHeight);
const std::size_t cell_count = grid.x_grid.size() * grid.z_grid.size();
grid.tx_distance_grids.assign(x_tx.size(), std::vector<double>(cell_count, 0.0));
grid.rx_distance_grids.assign(x_rx.size(), std::vector<double>(cell_count, 0.0));
grid.tx_distance_grids.assign(selection.x_tx.size(), std::vector<double>(cell_count, 0.0));
grid.rx_distance_grids.assign(selection.x_rx.size(), std::vector<double>(cell_count, 0.0));
for (std::size_t row = 0U; row < grid.z_grid.size(); ++row) {
const double z_value = grid.z_grid[row];
@@ -702,13 +740,19 @@ void normalize_pair_ascans(
const double x_value = grid.x_grid[col];
const auto cell_index = (row * grid.x_grid.size()) + col;
for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) {
grid.tx_distance_grids[tx_index][cell_index] =
std::hypot(x_value - x_tx[tx_index], z_value);
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],
z_value - selection.z_tx[tx_index]
);
}
for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) {
grid.rx_distance_grids[rx_index][cell_index] =
std::hypot(x_value - x_rx[rx_index], z_value);
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],
z_value - selection.z_rx[rx_index]
);
}
}
}
@@ -737,37 +781,50 @@ void normalize_pair_ascans(
[[nodiscard]] auto attenuation_components(
double r_tx,
double r_rx,
double z_m
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(z_m / (r_tx + 1e-12), 2.0) *
std::pow(z_m / (r_rx + 1e-12), 2.0);
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 std::vector<double>& x_tx,
const std::vector<double>& x_rx
const GeometrySelection& selection,
double imaging_plane_y_m
) -> std::pair<double, double> {
const double x_center = 0.5 * (x_tx[tx_index] + x_rx[rx_index]);
const double r_tx = std::hypot(x_center - x_tx[tx_index], kCompensationReferenceDepthM);
const double r_rx = std::hypot(x_center - x_rx[rx_index], kCompensationReferenceDepthM);
return attenuation_components(r_tx, r_rx, kCompensationReferenceDepthM);
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 z_m,
double dz_tx,
double dz_rx,
double geo_ref,
double angle_ref,
double range_power,
double angle_power
) -> double {
const auto [geo, angle] = attenuation_components(r_tx, r_rx, z_m);
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;
@@ -788,8 +845,8 @@ void normalize_pair_ascans(
const std::vector<SelectedTrace>& selected_traces,
const std::unordered_map<PairKey, AscanResult>& ascans_by_pair,
const GridDefinition& grid,
const std::vector<double>& x_tx,
const std::vector<double>& x_rx,
const GeometrySelection& selection,
double imaging_plane_y_m,
double velocity_mps,
double min_depth_m,
double max_depth_m,
@@ -821,11 +878,13 @@ void normalize_pair_ascans(
const auto [geo_ref, angle_ref] = attenuation_components_at_ref_depth(
trace.tx_local_index,
trace.rx_local_index,
x_tx,
x_rx
selection,
imaging_plane_y_m
);
const auto& tx_distances = grid.tx_distance_grids[trace.tx_local_index];
const auto& rx_distances = grid.rx_distance_grids[trace.rx_local_index];
const double z_tx_ant = selection.z_tx[trace.tx_local_index];
const double z_rx_ant = selection.z_rx[trace.rx_local_index];
for (std::size_t row = 0U; row < height; ++row) {
const double z_m = grid.z_grid[row];
@@ -833,6 +892,8 @@ void normalize_pair_ascans(
if (!in_depth_gate) {
continue;
}
const double dz_tx = z_m - z_tx_ant;
const double dz_rx = z_m - z_rx_ant;
for (std::size_t col = 0U; col < width; ++col) {
const auto cell_index = (row * width) + col;
@@ -847,7 +908,8 @@ void normalize_pair_ascans(
const double weight = compensation_weight(
r_tx,
r_rx,
z_m,
dz_tx,
dz_rx,
geo_ref,
angle_ref,
range_power,
@@ -1209,14 +1271,24 @@ void apply_depth_gate(
[[nodiscard]] auto bistatic_depth_signature(
const ObjectRecord& object,
const std::vector<SelectedTrace>& selected_traces,
const std::vector<double>& x_tx,
const std::vector<double>& x_rx
const GeometrySelection& selection,
double imaging_plane_y_m
) -> std::vector<double> {
std::vector<double> signature{};
signature.reserve(selected_traces.size());
for (const auto& trace : selected_traces) {
const double r_tx = std::hypot(object.x_m - x_tx[trace.tx_local_index], object.z_m);
const double r_rx = std::hypot(object.x_m - x_rx[trace.rx_local_index], object.z_m);
const auto tx = trace.tx_local_index;
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],
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],
object.z_m - selection.z_rx[rx]
);
signature.push_back(0.5 * (r_tx + r_rx));
}
return signature;
@@ -1225,13 +1297,13 @@ void apply_depth_gate(
void mark_sidelobe_candidates(
std::vector<ObjectRecord>& objects,
const std::vector<SelectedTrace>& selected_traces,
const std::vector<double>& x_tx,
const std::vector<double>& x_rx
const GeometrySelection& selection,
double imaging_plane_y_m
) {
std::vector<std::vector<double>> signatures{};
signatures.reserve(objects.size());
for (const auto& object : objects) {
signatures.push_back(bistatic_depth_signature(object, selected_traces, x_tx, x_rx));
signatures.push_back(bistatic_depth_signature(object, selected_traces, selection, imaging_plane_y_m));
}
for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
@@ -1554,7 +1626,8 @@ void add_bp_score_metrics(
}
normalize_pair_ascans(ascans_by_pair, velocity_mps, min_depth_m, max_depth_m);
const auto grid = build_grid(selection.x_tx, selection.x_rx, max_depth_m, kGridZMinM);
const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
const auto grid = build_grid(selection, max_depth_m, kGridZMinM, imaging_plane_y_m);
if (grid.x_grid.empty() || grid.z_grid.empty()) {
return results;
}
@@ -1563,8 +1636,8 @@ void add_bp_score_metrics(
selected_traces,
ascans_by_pair,
grid,
selection.x_tx,
selection.x_rx,
selection,
imaging_plane_y_m,
velocity_mps,
min_depth_m,
max_depth_m,
@@ -1582,7 +1655,7 @@ void add_bp_score_metrics(
auto objects = find_bp_objects(display_map, grid);
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.x_tx, selection.x_rx);
mark_sidelobe_candidates(objects, selected_traces, selection, imaging_plane_y_m);
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
+6 -1
View File
@@ -678,7 +678,12 @@ void apply_legacy_motion_correction(
return results;
}
const auto grid = build_grid(selection.x_tx, selection.x_rx, max_depth_m, kLegacyGridZMinM);
const auto grid = build_grid(
selection,
max_depth_m,
kLegacyGridZMinM,
static_cast<double>(live_config.gpr_imaging_plane_y_m)
);
if (grid.x_grid.empty() || grid.z_grid.empty()) {
return results;
}