sar3d: port the Multi_antenna_3D_clean_v1 notebook algorithm
The old sar3d core was a port of the superseded Horns_3D notebook and produced wrong volumes. The compute core now follows the reference notebook exactly: - segmented_interleaved frame timing rebuilt from per-trace capture stamps (uniform tx_per_segment split as the no-stamps fallback), with the coarse inter-segment dtau and the intra-sweep dtau applied as one phase ramp before the IFFT; - ground scene coordinates with the ground_depth_m lift (scene Y=0 is the ground), full-region grid instead of a window around a center depth, geo_ref compensation anchored on the ground plane, power 0.3; - notebook finalization in C++: |acc/cnt|, depth mask, normalization without the Y-edge planes, separable Gaussian over (Z, X), level; - CUDA kernel computes geometry in double: float32 loses ~3e-4 of an A-scan sample in tau and drifts ~1e-4 from the float64 reference. Live-config keys follow: the center_depth/half_width/grid_n window knobs are replaced by ground/voxel/y-range/margin/sigma/power/tx_per_segment. Parity against the notebook npz exports (frames 0034-0036 of the 20260922_mine_fast_true dataset): max|diff| <= 1.6e-6 off the depth-mask knife edge, identical argmax, level rel diff <= 7e-7. Verified via make sar3d_parity --dump-dir + scripts/sar3d_volume_compare.py; the executable spec lives in scripts/sar3d_reference_check.py (bit-tight against the npz, including the animation shifted-grid arithmetic). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
@@ -1,5 +1,6 @@
|
||||
#include "gpr/geometry.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace gpr {
|
||||
@@ -48,20 +49,53 @@ std::array<double, 3> point_to_scene(double x, double y, double z, const MotionC
|
||||
const SARConfig& sar) {
|
||||
if (!sar.coord_mode_ground) return {x, y, z};
|
||||
double p = mot.pitch_deg * M_PI / 180.0;
|
||||
double ys = y * std::cos(p) - z * std::sin(p);
|
||||
double ys = y * std::cos(p) - z * std::sin(p) + sar.ground_depth_m;
|
||||
double zs = y * std::sin(p) + z * std::cos(p);
|
||||
return {x, ys, zs};
|
||||
}
|
||||
|
||||
Grid build_grid_scene(const std::array<double, 3>& c, const SARConfig& sar) {
|
||||
Grid g;
|
||||
auto lin = [](double a, double b, int n, std::vector<double>& out) {
|
||||
double ground_plane_y(double depth, double ground_depth_m, double pitch_rad) {
|
||||
return (-ground_depth_m + depth * std::sin(pitch_rad)) / std::cos(pitch_rad);
|
||||
}
|
||||
|
||||
Grid build_full_region_grid(const GeometryConfig& geom, const BPConfig& bp, const SARConfig& sar,
|
||||
double pitch_deg) {
|
||||
// numpy: linspace(lo, hi, max(round(span/step)+1, 2)); nearbyint = банковское
|
||||
// округление python round()
|
||||
auto axis = [](double lo, double hi, double step, std::vector<double>& out) {
|
||||
int n = std::max((int)std::nearbyint((hi - lo) / step) + 1, 2);
|
||||
out.resize(n);
|
||||
for (int i = 0; i < n; ++i) out[i] = a + (b - a) * i / (double)(n - 1);
|
||||
for (int i = 0; i < n; ++i) out[i] = lo + (hi - lo) * i / (double)(n - 1);
|
||||
};
|
||||
lin(c[0] - sar.half_width_m[0], c[0] + sar.half_width_m[0], sar.grid[0], g.x);
|
||||
lin(c[1] - sar.half_width_m[1], c[1] + sar.half_width_m[1], sar.grid[1], g.y);
|
||||
lin(c[2] - sar.half_width_m[2], c[2] + sar.half_width_m[2], sar.grid[2], g.z);
|
||||
double x_lo = geom.x_tx[0], x_hi = geom.x_tx[0];
|
||||
for (double x : geom.x_tx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
|
||||
for (double x : geom.x_rx) { x_lo = std::min(x_lo, x); x_hi = std::max(x_hi, x); }
|
||||
x_lo -= sar.x_margin_m;
|
||||
x_hi += sar.x_margin_m;
|
||||
|
||||
Grid g;
|
||||
axis(x_lo, x_hi, sar.voxel_m[0], g.x);
|
||||
axis(sar.y_min_m, sar.y_max_m, sar.voxel_m[1], g.y);
|
||||
if (sar.coord_mode_ground) {
|
||||
// Радарная z линейна по Z сцены при фиксированной высоте:
|
||||
// z_rad(Y, Z=0) = -(Y - ground_depth)*sin(p), z = (depth - z_rad)/cos(p).
|
||||
double p = pitch_deg * M_PI / 180.0;
|
||||
double cos_p = std::cos(p), sin_p = std::sin(p);
|
||||
double z_lo = 0.0, z_hi = 0.0;
|
||||
bool first = true;
|
||||
for (double y : {sar.y_min_m, sar.y_max_m}) {
|
||||
for (double depth : {bp.min_depth, bp.max_depth}) {
|
||||
double z_rad0 = -(y - sar.ground_depth_m) * sin_p;
|
||||
double z = (depth - z_rad0) / cos_p;
|
||||
if (first) { z_lo = z_hi = z; first = false; }
|
||||
z_lo = std::min(z_lo, z);
|
||||
z_hi = std::max(z_hi, z);
|
||||
}
|
||||
}
|
||||
axis(z_lo, z_hi, sar.voxel_m[2], g.z);
|
||||
} else {
|
||||
axis(bp.min_depth, bp.max_depth, sar.voxel_m[2], g.z);
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user