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>
2026-09-25 16:42:01 +03:00
parent e9a0b43ed3
commit b8cb572bc4
17 changed files with 1264 additions and 353 deletions
@@ -4,7 +4,10 @@
 #include <bit>
 #include <cmath>
 #include <cstdint>
+#include <cstdio>
+#include <fstream>
 #include <iostream>
+#include <limits>
 #include <map>
 #include <optional>
 #include <stdexcept>
@@ -19,7 +22,6 @@ namespace radar::processing {
 namespace {

 constexpr std::size_t kAscanCacheLimit = 64U;
-constexpr double kVolumeEps = 1e-12;

 void hash_mix(std::size_t& seed, std::uint64_t value) {
    seed ^= value + 0x9e3779b97f4a7c15ULL + (seed << 6U) + (seed >> 2U);
@@ -130,6 +132,7 @@ struct Selection {
    bp.pair_normalize = live_config.sar3d_normalize_pairs;
    bp.pair_norm_depth_min = bp.min_depth;
    bp.pair_norm_depth_max = bp.max_depth;
+    bp.comp_range_power = static_cast<double>(live_config.sar3d_comp_range_power);
    return bp;
 }

@@ -137,9 +140,13 @@ struct Selection {
    gpr::MotionConfig motion{};
    motion.speed_m_s = static_cast<double>(live_config.gpr_speed_m_s);
    motion.look_angle_deg = static_cast<double>(live_config.gpr_look_angle_deg);
-    motion.pitch_deg = static_cast<double>(live_config.sar3d_pitch_deg);
+    // Notebook BP_GROUND_PITCH_DEG = None -> the frame pitch follows the look angle.
+    motion.pitch_deg = live_config.sar3d_pitch_override_enabled
+                           ? static_cast<double>(live_config.sar3d_pitch_deg)
+                           : static_cast<double>(live_config.gpr_look_angle_deg);
    motion.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
    motion.apply_freq_phase_correction = live_config.gpr_apply_freq_phase_correction;
+    motion.tx_per_segment = static_cast<int>(std::max<std::uint32_t>(1U, live_config.sar3d_tx_per_segment));
    return motion;
 }

@@ -147,20 +154,35 @@ struct Selection {
    gpr::SARConfig sar{};
    sar.frame_radius = static_cast<int>(std::max<std::uint32_t>(0U, live_config.sar3d_frame_radius));
    sar.edge_frame_weight = static_cast<double>(live_config.sar3d_edge_frame_weight);
-    sar.half_width_m = {
-        static_cast<double>(live_config.sar3d_half_width_x_m),
-        static_cast<double>(live_config.sar3d_half_width_y_m),
-        static_cast<double>(live_config.sar3d_half_width_z_m),
-    };
-    sar.grid = {
-        static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_nx)),
-        static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_ny)),
-        static_cast<int>(std::max<std::uint32_t>(2U, live_config.sar3d_grid_nz)),
+    sar.ground_depth_m = static_cast<double>(live_config.sar3d_ground_depth_m);
+    sar.x_margin_m = static_cast<double>(live_config.sar3d_x_margin_m);
+    sar.y_min_m = static_cast<double>(live_config.sar3d_y_min_m);
+    sar.y_max_m = static_cast<double>(live_config.sar3d_y_max_m);
+    sar.voxel_m = {
+        static_cast<double>(live_config.sar3d_voxel_x_m),
+        static_cast<double>(live_config.sar3d_voxel_y_m),
+        static_cast<double>(live_config.sar3d_voxel_z_m),
    };
+    sar.norm_edge_margin_m = static_cast<double>(live_config.sar3d_norm_edge_margin_m);
+    sar.smooth_sigma = static_cast<double>(live_config.sar3d_smooth_sigma);
    sar.use_compensation = live_config.sar3d_use_compensation;
    return sar;
 }

+// Hardware Tx order: first occurrence of each output in run_combos. Deliberately
+// ignores the live Tx selection so segment/slot assignment stays a hardware
+// property (notebook tx_order_full).
+[[nodiscard]] auto tx_order_full(const config::RunConfig& run_config) -> std::vector<int> {
+    std::vector<int> order{};
+    for (const auto& combo : run_config.run_combos) {
+        const int output = static_cast<int>(combo.output_pos);
+        if (std::find(order.begin(), order.end(), output) == order.end()) {
+            order.push_back(output);
+        }
+    }
+    return order;
+}
+
 [[nodiscard]] auto ascan_fingerprint(
    const Selection& selection,
    const gpr::GeometryConfig& geom,
@@ -184,6 +206,7 @@ struct Selection {
    hash_mix(seed, motion.look_angle_deg);
    hash_mix(seed, motion.direction_sign);
    hash_mix(seed, static_cast<std::uint64_t>(motion.apply_freq_phase_correction ? 1U : 0U));
+    hash_mix(seed, static_cast<std::uint64_t>(motion.tx_per_segment));
    return seed;
 }

@@ -215,6 +238,7 @@ struct Selection {
    hash_mix(seed, bp.comp_weight_max);
    hash_mix(seed, bp.comp_ref_depth);
    hash_mix(seed, motion.pitch_deg);
+    hash_mix(seed, sar.ground_depth_m);
    hash_mix(seed, static_cast<std::uint64_t>(sar.use_compensation ? 1U : 0U));
    return seed;
 }
@@ -228,14 +252,80 @@ struct Selection {
    return static_cast<double>(collection.monotonic_ns) * 1e-9;
 }

-// One Tx's interleaved sweep spans the whole capture window, so the per-Tx
-// sweep time is capture_span / N_tx (same derivation as the coherent BP path).
-[[nodiscard]] auto tx_sweep_time_s(const ipc::PreprocessedCollection& collection, int n_tx) -> double {
-    if (collection.capture_end_ns <= collection.capture_start_ns || n_tx <= 0) {
-        return 0.0;
+// Real frame timing from the per-trace capture stamps: traces measured in one
+// segment share one window on both producers, so the first window seen per Tx
+// reconstructs the segments exactly (notebook snapshot_acquisition_timing).
+// Invalid when any trace carries the zero "unmeasured" sentinel.
+struct SnapshotTiming {
+    gpr::TxWindows windows{};    // seconds relative to the first trace start
+    double acq_s = 0.0;
+    double center_abs_s = 0.0;   // absolute acquisition centre (frame time)
+    bool valid = false;
+};
+
+[[nodiscard]] auto snapshot_timing(const ipc::PreprocessedCollection& collection) -> SnapshotTiming {
+    SnapshotTiming timing{};
+    if (collection.traces.empty()) {
+        return timing;
    }
-    const double span_s = static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9;
-    return span_s / static_cast<double>(n_tx);
+    std::map<int, std::pair<std::uint64_t, std::uint64_t>> by_tx{};
+    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) {
+            return timing;
+        }
+        by_tx.try_emplace(
+            static_cast<int>(trace.combo.output_pos),
+            std::pair{trace.capture_start_ns, trace.capture_end_ns}
+        );
+        first = std::min(first, trace.capture_start_ns);
+        last = std::max(last, trace.capture_end_ns);
+    }
+    for (const auto& [tx, window] : by_tx) {
+        timing.windows[tx] = {
+            static_cast<double>(window.first - first) * 1e-9,
+            static_cast<double>(window.second - first) * 1e-9,
+        };
+    }
+    timing.acq_s = static_cast<double>(last - first) * 1e-9;
+    timing.center_abs_s = 0.5 * (static_cast<double>(first) + static_cast<double>(last)) * 1e-9;
+    timing.valid = true;
+    return timing;
+}
+
+// Debug dump for the parity tool: raw float32 volume [iy][iz][ix] plus a json
+// header with the axes and level.
+void dump_volume(
+    const std::string& dir,
+    std::uint64_t collection_id,
+    const gpr::FinishedVolume& finished,
+    const gpr::Grid& grid
+) {
+    char name[64];
+    std::snprintf(name, sizeof(name), "/sar3d_volume_%llu",
+                  static_cast<unsigned long long>(collection_id));
+    const std::string base = dir + name;
+    std::ofstream bin(base + ".f32", std::ios::binary | std::ios::trunc);
+    bin.write(
+        reinterpret_cast<const char*>(finished.volume.data()),
+        static_cast<std::streamsize>(finished.volume.size() * sizeof(float))
+    );
+    std::ofstream meta(base + ".json", std::ios::trunc);
+    meta.precision(17);
+    meta << "{\"nx\": " << finished.NX << ", \"ny\": " << finished.NY
+         << ", \"nz\": " << finished.NZ << ", \"level\": " << finished.level;
+    const auto write_axis = [&meta](const char* key, const std::vector<double>& axis) {
+        meta << ", \"" << key << "\": [";
+        for (std::size_t index = 0U; index < axis.size(); ++index) {
+            meta << (index > 0U ? "," : "") << axis[index];
+        }
+        meta << "]";
+    };
+    write_axis("x_m", grid.x);
+    write_axis("y_m", grid.y);
+    write_axis("z_m", grid.z);
+    meta << "}\n";
 }

 [[nodiscard]] auto build_image_payload(
@@ -285,14 +375,26 @@ struct Sar3dProcessor::State {
        const Selection& selection,
        const gpr::GeometryConfig& geom,
        const gpr::BPConfig& bp,
-        gpr::MotionConfig motion
+        const gpr::MotionConfig& motion,
+        const std::vector<int>& tx_order
    ) -> const CacheEntry& {
        if (const auto found = ascan_cache.find(collection.collection_id); found != ascan_cache.end()) {
            return found->second;
        }

        CacheEntry entry{};
-        entry.time_s = collection_time_s(collection);
+        auto timing = snapshot_timing(collection);
+        // The segmented model needs a window for every selected Tx; fall back to
+        // the uniform split otherwise (notebook median model).
+        if (timing.valid) {
+            for (const auto output : geom.output_idx) {
+                if (timing.windows.find(output) == timing.windows.end()) {
+                    timing.valid = false;
+                    break;
+                }
+            }
+        }
+        entry.time_s = timing.valid ? timing.center_abs_s : collection_time_s(collection);

        std::map<gpr::PairKey, std::vector<gpr::c128>> s21{};
        std::map<gpr::PairKey, std::vector<double>> freq{};
@@ -317,9 +419,25 @@ struct Sar3dProcessor::State {
        }

        if (s21.size() == selection.inputs.size() * selection.outputs.size()) {
-            motion.tx_sweep_time_s = tx_sweep_time_s(collection, geom.n_tx);
-            const auto timestamps = gpr::compute_pair_timestamps(motion, geom);
+            gpr::TxWindows windows{};
+            double acq_s = 0.0;
+            if (timing.valid) {
+                windows = std::move(timing.windows);
+                acq_s = timing.acq_s;
+            } else {
+                if (collection.capture_end_ns > collection.capture_start_ns) {
+                    acq_s = static_cast<double>(collection.capture_end_ns - collection.capture_start_ns) * 1e-9;
+                }
+                auto order = tx_order;
+                for (const auto output : geom.output_idx) {
+                    if (std::find(order.begin(), order.end(), output) == order.end()) {
+                        order.push_back(output);
+                    }
+                }
+                windows = gpr::uniform_tx_windows(order, acq_s, motion.tx_per_segment);
+            }
            try {
+                const auto timestamps = gpr::compute_pair_timestamps(windows, acq_s, tx_order, geom, motion);
                entry.ascans = std::make_shared<gpr::AScanSet>(gpr::preprocess_snapshot(
                    s21, freq, /*background=*/nullptr, motion, geom, bp, timestamps, bp.pair_normalize
                ));
@@ -399,12 +517,14 @@ auto Sar3dProcessor::process_collection(
        }
        window.push_back(&collection);

+        const auto tx_order = tx_order_full(run_config);
        std::vector<std::shared_ptr<gpr::AScanSet>> ascan_sets{};
        std::vector<double> frame_times{};
        ascan_sets.reserve(window.size());
        frame_times.reserve(window.size());
        for (const auto* entry_collection : window) {
-            const auto& entry = state_->get_or_compute_ascans(*entry_collection, selection, geom, bp, motion);
+            const auto& entry =
+                state_->get_or_compute_ascans(*entry_collection, selection, geom, bp, motion, tx_order);
            if (entry.ascans == nullptr) {
                continue;
            }
@@ -450,14 +570,17 @@ auto Sar3dProcessor::process_collection(
            state_->engine_fp = engine_fp;
        }

-        const auto center_scene =
-            gpr::point_to_scene(0.0, 0.0, static_cast<double>(live_config.sar3d_center_depth_m), motion, sar);
-        const gpr::Grid grid = gpr::build_grid_scene(center_scene, sar);
-        const gpr::VolumeRaw volume = state_->engine->aperture_volume(frames, grid);
+        const gpr::Grid grid = gpr::build_full_region_grid(geom, bp, sar, motion.pitch_deg);
+        const gpr::VolumeRaw raw = state_->engine->aperture_volume(frames, grid);
+        // Full notebook finalization: |acc/cnt|, depth gate, normalization
+        // without the Y-edge planes, Gaussian smoothing over (Z, X), clip.
+        const gpr::FinishedVolume finished =
+            gpr::finish_volume(raw, grid, motion.pitch_deg, bp, sar);
+
+        if (!live_config.sar3d_volume_dump_dir.empty()) {
+            dump_volume(live_config.sar3d_volume_dump_dir, collection.collection_id, finished, grid);
+        }

-        // Finalization (port of gpr_core sar_finish_volume, minus smoothing):
-        // |acc/(cnt+eps)|, ground-coordinate depth gate, normalization to peak.
-        //
        // Two projections match the legacy render conventions (X across,
        // Z range/"вдаль", Y up):
        // - top view `sar3d_proj_xz`: max over Y, axes X (horizontal) / Z;
@@ -466,12 +589,9 @@ auto Sar3dProcessor::process_collection(
        //   `sar3d_bottom_view_deg` around Y: 0° = front view (u = X, max
        //   along Z), 90° = side view (u = Z, max along X), intermediate
        //   angles project along the rotated perpendicular.
-        const int nx = volume.NX;
-        const int ny = volume.NY;
-        const int nz = volume.NZ;
-        const double pitch_rad = motion.pitch_deg * M_PI / 180.0;
-        const double cos_p = std::cos(pitch_rad);
-        const double sin_p = std::sin(pitch_rad);
+        const int nx = finished.NX;
+        const int ny = finished.NY;
+        const int nz = finished.NZ;

        const double theta_deg = std::clamp(static_cast<double>(live_config.sar3d_bottom_view_deg), 0.0, 90.0);
        const double theta_rad = theta_deg * M_PI / 180.0;
@@ -501,24 +621,18 @@ auto Sar3dProcessor::process_collection(
        const double u_start = u_axis.front();
        const double inv_du = nu > 1 ? static_cast<double>(nu - 1) / (u_axis.back() - u_start) : 0.0;

+        // The volume is already normalized to ~[0, 1] by finish_volume, so the
+        // projections are published as-is; the scalar payload carries `level`
+        // (the absolute scale the volume was divided by).
        std::vector<float> projection_xz(static_cast<std::size_t>(nz) * nx, 0.0F);
        std::vector<float> projection_bottom(static_cast<std::size_t>(ny) * nu, 0.0F);
-        float peak = 0.0F;
        for (int iy = 0; iy < ny; ++iy) {
            for (int iz = 0; iz < nz; ++iz) {
-                const double z_rad = -grid.y[iy] * sin_p + grid.z[iz] * cos_p;
-                const bool depth_ok = z_rad >= bp.min_depth && z_rad <= bp.max_depth;
-                if (!depth_ok) {
-                    continue;
-                }
                const std::size_t row = (static_cast<std::size_t>(iy) * nz + iz) * nx;
                const std::size_t bottom_row = static_cast<std::size_t>(iy) * nu;
                const double z_sin_t = grid.z[iz] * sin_t;
                for (int ix = 0; ix < nx; ++ix) {
-                    const std::size_t voxel = row + static_cast<std::size_t>(ix);
-                    const float value = std::abs(volume.acc[voxel]) /
-                        (volume.cnt[voxel] + static_cast<float>(kVolumeEps));
-                    peak = std::max(peak, value);
+                    const float value = finished.volume[row + static_cast<std::size_t>(ix)];
                    auto& xz_cell = projection_xz[static_cast<std::size_t>(iz) * nx + ix];
                    xz_cell = std::max(xz_cell, value);
                    int bin = ix;
@@ -534,14 +648,6 @@ auto Sar3dProcessor::process_collection(
                }
            }
        }
-        if (peak > 0.0F) {
-            for (auto& value : projection_xz) {
-                value /= peak;
-            }
-            for (auto& value : projection_bottom) {
-                value /= peak;
-            }
-        }

        results.collection_payloads.push_back(
            build_image_payload("sar3d_proj_xz", grid.x, grid.z, std::move(projection_xz))
@@ -552,7 +658,7 @@ auto Sar3dProcessor::process_collection(
        ipc::ResultPayload peak_payload{};
        peak_payload.processing_name = "sar3d_peak";
        peak_payload.kind = ipc::ResultKind::ScalarF32;
-        peak_payload.scalar_value = peak;
+        peak_payload.scalar_value = static_cast<float>(finished.level);
        results.collection_payloads.push_back(std::move(peak_payload));
    } catch (const std::exception& exc) {
        // A CUDA/NVRTC or data error must not kill the data_processor loop: