diff --git a/.agent/work-plans/issue-87/progress.md b/.agent/work-plans/issue-87/progress.md new file mode 100644 index 0000000..0e9433d --- /dev/null +++ b/.agent/work-plans/issue-87/progress.md @@ -0,0 +1,56 @@ +--- +issue: 87 +--- + +# Issue #87 — Tier-2 TL-removed (depth-transferable) backscatter angular-response curve + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-06-28 20:15 +0000 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: changes-requested + +**Branch**: feature/issue-87 at `24a0187` +**Mode**: pre-push +**Depth**: Deep (reason: substantive ADR-0007 addendum + cross-cutting estimator change threading a new field through a pack(1) binary-raster struct) +**Must-fix**: 2 | **Suggestions**: 3 +**Round**: 1 | **Ship**: continue — one genuine, cross-pass-confirmed correctness concern (live-path R ≠ calibration R) warrants a fix or explicit bounding before the transferability claim holds. + +### Findings +- [ ] (must-fix) Stale inverted-sign formula in the `range` field doc comment (`corrected = raw - (40*log10(R)+2*alpha*R) - residual`); validated code/ADR use `raw + TL(R) - residual`. Commit 24a0187 fixed the sign everywhere except this header — `include/cube_bathymetry/hypothesis.h:55` +- [ ] (must-fix) Live-path `slant_range = sqrt(x²+y²+z²)` = `R·√(1+sin²tx·sin²rx)` ≠ offline/Python-calibration `R = twtt·c/2` when tx_angle≠0 (cross-pass confirmed, Lens A+B); silently biases the live correction and breaks live/offline equivalence. Comment "norm IS the slant range" is inaccurate. Reconcile R, or bound+document the limitation — `src/cube_bathymetry_node.cpp:973-975` (cf. `sounding.h:52-54`, `import_bag_main.cpp:604`) +- [ ] (suggestion) `freqs` accumulated unconditionally even in tier-1 (unused); guard on `remove_tl` — `scripts/derive_angular_response.py:168-170` +- [ ] (suggestion) Python drops non-positive/NaN-range beams from bins entirely while C++ retains them (TL-skipped); note the intentional asymmetry — `scripts/derive_angular_response.py:189-196` / `src/node.cpp:397` +- [ ] (suggestion) `parse_args` missing a docstring (flake8-docstrings D103) — `scripts/derive_angular_response.py:58` + +### Verified correct (both adversarial passes, no action) +- TL sign (added-back/TVG-style) consistent across Python, CSV, and `node.cpp:401`. +- Francois-Garrison freshwater A3/P3 coefficients + T>20/≤20 split are standard; 500 kHz/24 °C → ~0.049 dB/m verified by hand. Units (Hz→kHz, dB/m) consistent. +- Absorption (linear, per-bin via mean R) vs spreading (non-linear log, per-beam) aggregation is identical Python↔C++ by construction; α read verbatim in C++. +- `DepthAndUncertainty` 16→20 byte growth is layout-safe (only `sizeof()` uses are bag_to_geotiff RasterIO strides that auto-track; all else by-name). +- `range` threaded through every update/queueEstimate/recordBeam/queueFlush path; no silent drop. +- Tier-1 backward compatibility preserved (`apply_tl = apply_ara && tl_removed`; defaulted params/args; old loader API retained). Header parser tolerant; tests cover edge cases. + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-06-29 03:56 +0000 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: approved + +**Branch**: feature/issue-87 at `03cf523` +**Mode**: pre-push +**Depth**: Deep (reason: substantive ADR-0007 addendum + cross-cutting estimator change threading a new field through a pack(1) binary-raster struct) +**Must-fix**: 0 | **Suggestions**: 2 +**Round**: 2 | **Ship**: recommended — both round-1 must-fixes resolved (sign comment fixed; live-R bounded+documented for the M3 tx=0 case, the resolution round-1 explicitly permitted); two independent adversarial lenses + static analysis surfaced 0 new must-fix. + +### Findings +- [ ] (suggestion) Optional producer-frame assert on live-path `R = sqrt(x²+y²+z²)` — verified correct for the M3 (sensor-frame norm, tx=0) and honestly documented; an assert would harden against a future source emitting the cloud in a vehicle/base frame — `src/cube_bathymetry_node.cpp:977` +- [ ] (suggestion) Optional one-shot warn when tier-2 is active but the range-present beam fraction is low; the documented retain-vs-drop asymmetry (C++ keeps NaN/non-positive-range beams as residual-only; Python drops them) is then load-bearing — `src/node.cpp:393-403` + +### Verified resolved (round-1 must-fixes) +- Stale inverted-sign formula in `hypothesis.h` range-field doc now matches the validated `raw + (40*log10(R) + 2*alpha*R) - residual` model — `include/cube_bathymetry/hypothesis.h:54`. +- Live-path R vs offline-R discrepancy: the inaccurate "norm IS the slant range" comment (present at `24a0187`) replaced with a bounded, M3-tx=0-honest derivation; offline import uses `twtt*c/2` so live/offline match for the M3 — `src/cube_bathymetry_node.cpp:969-977`. + +### Verified correct (no action, round 2) +- Round-1 suggestions also addressed: `freqs` accumulation now guarded on `--remove-tl`; `parse_args` docstring added; retain-vs-drop asymmetry documented. +- flake8 (ament profile) clean; `py_compile` OK. `pack(1)` 16→20 B growth layout-safe — `bag_to_geotiff.cpp:707` strides by `sizeof(DepthAndUncertainty)`; no whole-struct persistence / on-disk format break. No governance concerns (ADR-0007 addendum extended coherently; consequences handled; tests cover new tier-2 paths). diff --git a/cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md b/cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md index 95b66f8..73e7e97 100644 --- a/cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md +++ b/cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md @@ -88,15 +88,71 @@ sonar. The `rx_angles` sign/zero convention was verified against symmetry) plus curve-loader/mode-parse tests; the 3 pre-existing default-path intensity assertions are unchanged (default `None` ⇒ `corrected == raw`). +## Tier-2 addendum — TL-removed, depth-transferable curve (cube_bathymetry#87) + +The tier-1 curve above bins backscatter by `|rx_angle|` only, so per-beam range +(spreading + absorption) is baked into the curve and it is **depth-regime +specific** (must be re-derived per survey). Tier-2 removes the per-beam **2-way +transmission loss** by **compensating (adding it back)** before the angular +response is characterized and applied — a distant return lost more energy, so it +is boosted to recover range-independent backscatter (TVG-style) — so the residual +curve becomes **range/depth transferable**: + +`corrected = raw + TL(R) − residualCurve(|beam_angle|)`, +`TL(R) = 40·log₁₀(R) + 2·α·R` (R = per-beam slant range `twtt·c/2`, m). + +> Sign note: TL is **added back**, not subtracted. An earlier draft subtracted it +> (making far beams dimmer → steeper curve + larger cross-bag spread); the +> validated fix compensates the loss, which flattens the curve and shrinks the +> spread. + +Design decisions: + +1. **The curve file is self-describing — one correction mode, not two.** The + `Empirical` mode is unchanged; the estimator applies TL **iff the loaded curve + says so**. The curve CSV header gains `# tl_removed: true` and + `# absorption_db_per_m: <α>` (plus `# water_temp_c` / `# tl_model` provenance). + Tier-1 curves carry `tl_removed: false` (or omit the lines) and keep working + unchanged — fully backward compatible. +2. **α lives only in Python.** The Francois-Garrison freshwater absorption is + computed once by `derive_angular_response.py` (`--remove-tl --water-temp-c`) + and written into the header as a scalar. The C++ estimator reads that scalar + verbatim and never recomputes α, so Python and C++ apply an **identical** TL by + construction (a divergence would silently corrupt the correction). At 500 kHz / + 24 °C / fresh water α ≈ 0.049 dB/m. +3. **Per-beam slant range R is a new sufficient statistic**, threaded exactly like + `beam_angle`: `Sounding::slant_range` → `DepthAndUncertainty::range` (the + pack(1) raster struct grows to 20 bytes; layout-safe because every raster read + strides by `sizeof(DepthAndUncertainty)`) → `BeamIntensitySample::range`, + recorded on the winning depth hypothesis. The offline importer threads R from + the `Sounding` detections ctor; the live node recovers R as the norm of the + sensor-frame `/soundings` point (no new cloud field). A NaN / non-positive R + skips the TL term (no log of a non-positive range). +4. **Fresh water only.** The salinity (boric-acid + MgSO₄) seawater absorption + terms are out of scope for this issue; `--salinity > 0` is rejected by the tool. +5. **Multi-bag derivation.** `derive_angular_response.py` now accepts multiple + bags (`nargs='+'`) and merges the per-bin sums — a real survey calibration + spans many bags. + +This still does **not** implement the full GeoCoder (insonified-area + +beam-pattern), which is the deferred tier-3. The TVG/absorption/frequency state +ultimately belongs in `SonarInfo` (unh_marine_autonomy#240); α is computed +locally meanwhile. + ## Deferred (explicit follow-ups) -- **Full radiometric GeoCoder** — insonified-area, beam-pattern, TVG residual, - and the depth/slope incidence term (ADR-0007 D3 / cube_bathymetry#15 / #59). - The empirical curve absorbs the aggregate angular falloff for a flat bottom; - the slope-aware incidence correction remains future work, gated on the +- **Full radiometric GeoCoder (tier-3)** — insonified-area, beam-pattern, TVG + residual, and the depth/slope incidence term (ADR-0007 D3 / cube_bathymetry#15 + / #59). The empirical curve (tier-1) absorbs the aggregate angular falloff for a + flat bottom; tier-2 makes it range/depth transferable; the slope-aware incidence + correction and the area/beam-pattern terms remain future work, gated on the predicted-surface producer (#59). **Follow-up to file:** author the full, canonical `docs/decisions/0007-mbes-backscatter-store.md` document (this addendum folds into it). +- **TVG/absorption/frequency state in SonarInfo** (unh_marine_autonomy#240): the + M3's already-applied TVG is the one genuine remaining unknown; the tier-1-vs-2 + comparison resolves it empirically. Until then α is computed locally from the + per-ping frequency + water temperature. - **Intensity domain (dB vs linear) as a first-class sonar property.** The correction assumes dB (a subtraction). Sonars reporting linear intensity would need a divide, or a domain conversion at ingest. **Follow-up to file:** model diff --git a/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h b/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h index c8e9cb3..c8e2ba6 100644 --- a/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h +++ b/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h @@ -37,15 +37,41 @@ namespace cube bool parseBackscatterAngleCorrection( const std::string & text, BackscatterAngleCorrection & out); +/// A loaded angular-response curve plus its self-describing TL provenance +/// (cube_bathymetry#87). `points` is the ascending {abs_angle_deg, +/// db_relative_to_nadir} curve. When `tl_removed` is true the curve is a TL- +/// REMOVED residual (tier-2) and the estimator must remove the per-beam 2-way +/// transmission loss `40*log10(R) + 2*alpha*R` (alpha == `absorption_db_per_m`) +/// before subtracting the residual. A tier-1 curve has `tl_removed == false` +/// and `absorption_db_per_m == 0`, fully backward compatible. + struct AngularResponseCurve + { + std::vector < std::pair < float, float >> points; + bool tl_removed = false; + float absorption_db_per_m = 0.0f; + }; + /// Load an empirical angular-response curve from a CSV file matching the seed at /// ~/data/logs/analysis/m3_angular_response_curve.csv: a header line then rows /// `abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir`. Reads the first /// (abs_angle_deg_center) and fourth (db_relative_to_nadir) columns into ascending /// {abs_angle_deg, db_relative_to_nadir} pairs (sorted by angle). /// +/// Also parses the optional self-describing TL header comments written by the +/// derive tool (cube_bathymetry#87): +/// `# tl_removed: true|false` +/// `# absorption_db_per_m: ` +/// Their absence yields tier-1 defaults (tl_removed=false, absorption=0), so a +/// tier-1 curve keeps loading unchanged. The scalar `absorption_db_per_m` is read +/// verbatim (the C++ estimator never recomputes alpha). +/// /// Comment lines (`#`...), blank lines, the header row, and rows that fail to -/// parse are skipped. A missing/unreadable file yields an empty vector (the +/// parse are skipped. A missing/unreadable file yields an empty curve (the /// Empirical correction then degrades to a no-op -- callers should warn). + AngularResponseCurve loadAngularResponseCurveWithHeader(const std::string & path); + +/// Backward-compatible convenience: the curve `points` only (drops the TL +/// provenance). Equivalent to `loadAngularResponseCurveWithHeader(path).points`. std::vector < std::pair < float, float >> loadAngularResponseCurve(const std::string & path); } // namespace cube diff --git a/cube_bathymetry/include/cube_bathymetry/common.h b/cube_bathymetry/include/cube_bathymetry/common.h index 7cc44c1..c37d490 100644 --- a/cube_bathymetry/include/cube_bathymetry/common.h +++ b/cube_bathymetry/include/cube_bathymetry/common.h @@ -85,17 +85,28 @@ namespace cube /// Per-beam receive/steering angle (radians) accompanying the intensity, the /// {raw intensity, angle} sufficient-statistics pair (ADR-0007 D3). NaN when - /// not reported. NOTE: extending this #pragma pack(push,1) struct from 8 to - /// 16 bytes is layout-safe -- no caller depends on sizeof(DepthAndUncertainty) - /// (the only sizeof uses are raster-band strides, 2*sizeof(float)). + /// not reported. float beam_angle; + /// Per-beam slant range R (meters) accompanying the intensity, carried through + /// the median queue bound to its depth so the tier-2 backscatter TL correction + /// (cube_bathymetry#87) reads the same beam's range. NaN when not reported. + /// + /// LAYOUT NOTE: this #pragma pack(push,1) struct is now 20 bytes (5 floats). + /// The only sizeof uses are raster-band strides that read the first two floats + /// (depth, uncertainty) as bands using sizeof(DepthAndUncertainty) as the + /// element stride (e.g. bag_to_geotiff.cpp RasterIO). Appending floats at the + /// END is layout-safe ONLY because every raster read uses + /// sizeof(DepthAndUncertainty) (NOT a hardcoded 8/16); verified for #87. + float range; + DepthAndUncertainty(float depth = std::numeric_limits < float > ::quiet_NaN(), float uncertainty = std::numeric_limits < float > ::quiet_NaN(), float intensity = std::numeric_limits < float > ::quiet_NaN(), - float beam_angle = std::numeric_limits < float > ::quiet_NaN()) + float beam_angle = std::numeric_limits < float > ::quiet_NaN(), + float range = std::numeric_limits < float > ::quiet_NaN()) : depth(depth), uncertainty(uncertainty), intensity(intensity), - beam_angle(beam_angle) { + beam_angle(beam_angle), range(range) { } }; #pragma pack(pop) diff --git a/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h b/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h index 7593df5..4976b6e 100644 --- a/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h +++ b/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h @@ -57,9 +57,18 @@ namespace cube /// every owned GeoGrid holds by const reference -- so the setting reaches all /// existing and future grids. Call AFTER construction and BEFORE/at processing. /// An Empirical mode with an empty curve is a no-op (the caller should warn). + /// + /// @p tl_removed / @p absorption_db_per_m carry the tier-2 TL provenance + /// (cube_bathymetry#87): when @p tl_removed is true the @p curve is a TL- + /// removed residual and the estimator removes `40*log10(R) + 2*alpha*R` per + /// beam (alpha == @p absorption_db_per_m) before subtracting the residual. + /// Both default to the tier-1 values (false / 0), so existing callers are + /// unchanged. void setBackscatterCorrection( BackscatterAngleCorrection mode, - std::vector < std::pair < float, float >> curve); + std::vector < std::pair < float, float >> curve, + bool tl_removed = false, + float absorption_db_per_m = 0.0f); /// Return the grids within the bounds, creating new ones if necessary std::vector < std::shared_ptr < diff --git a/cube_bathymetry/include/cube_bathymetry/hypothesis.h b/cube_bathymetry/include/cube_bathymetry/hypothesis.h index 8b6e0e4..4468aa3 100644 --- a/cube_bathymetry/include/cube_bathymetry/hypothesis.h +++ b/cube_bathymetry/include/cube_bathymetry/hypothesis.h @@ -24,6 +24,7 @@ #define CUBE_BATHYMETRY__HYPOTHESIS_H_ #include +#include #include #include #include "cube_bathymetry/parameters.h" @@ -48,6 +49,15 @@ namespace cube /// treats a NaN angle as "no angle correction available" and emits that beam /// uncorrected. float beam_angle; + + /// Per-beam slant range R from the sonar head to the touchdown, in meters, + /// for the tier-2 backscatter 2-way transmission-loss correction + /// (cube_bathymetry#87): `corrected = raw + (40*log10(R) + 2*alpha*R) - + /// residualCurve(|angle|)` (the TL is ADDED BACK to compensate the loss). NaN + /// (or non-positive) when not reported; the TL + /// term is then skipped (identity) so the beam is corrected by the residual + /// angular-response curve alone (tier-1 behavior). + float range = std::numeric_limits < float > ::quiet_NaN(); }; /// Depth hypothesis structure used to maintain a current track on the depth @@ -115,8 +125,12 @@ namespace cube /// /// A NaN raw_intensity is skipped (a source that omits intensities must never /// inject a phantom sample); a NaN beam_angle is retained (the beam is - /// still a valid intensity sample, merely uncorrectable for angle). - void recordBeam(float raw_intensity, float beam_angle); + /// still a valid intensity sample, merely uncorrectable for angle). A NaN / + /// non-positive range is also retained (the beam is a valid intensity sample, + /// merely uncorrectable for the tier-2 TL term, cube_bathymetry#87). + void recordBeam( + float raw_intensity, float beam_angle, + float range = std::numeric_limits < float > ::quiet_NaN()); /// Current depth mean estimate double current_estimate; diff --git a/cube_bathymetry/include/cube_bathymetry/node.h b/cube_bathymetry/include/cube_bathymetry/node.h index 95b2837..deff492 100644 --- a/cube_bathymetry/include/cube_bathymetry/node.h +++ b/cube_bathymetry/include/cube_bathymetry/node.h @@ -113,9 +113,12 @@ namespace cube /// backscatter association tracks the depth association (ADR-0007 D2/D3). /// beam_angle: per-beam receive/steering angle (radians, NaN when absent), /// the angle half of the {raw intensity, angle} sufficient-stats pair. + /// range: per-beam slant range (m, NaN when absent), for the tier-2 TL + /// correction (cube_bathymetry#87). bool update( float depth, float variance, const Parameters & parameters, - float intensity = std::nan(""), float beam_angle = std::nan("")); + float intensity = std::nan(""), float beam_angle = std::nan(""), + float range = std::nan("")); /// Find the closest matching hypothesis in the current linked list. /// This computes the normalised absolute error between one-step @@ -164,9 +167,12 @@ namespace cube /// intensity/beam_angle: the {raw intensity, angle} pair for this beam, /// carried on the queue entry bound to its depth so the median sort never /// mismatches a depth with a foreign intensity (ADR-0007 D3). + /// range: per-beam slant range (m, NaN when absent), carried alongside so the + /// tier-2 TL correction reads the same beam's range (cube_bathymetry#87). bool queueEstimate( float depth, float variance, const Parameters & parameters, - float intensity = std::nan(""), float beam_angle = std::nan("")); + float intensity = std::nan(""), float beam_angle = std::nan(""), + float range = std::nan("")); /* Routine: cube_node_extract_depth_unct * Purpose: Extract depth and uncertainty of current best estimate diff --git a/cube_bathymetry/include/cube_bathymetry/parameters.h b/cube_bathymetry/include/cube_bathymetry/parameters.h index cbb5aa7..2d94b74 100644 --- a/cube_bathymetry/include/cube_bathymetry/parameters.h +++ b/cube_bathymetry/include/cube_bathymetry/parameters.h @@ -190,6 +190,24 @@ namespace cube /// CSV. Empty -> the Empirical correction is a no-op (logged as a warning at /// configure time). Used only when backscatter_angle_correction == Empirical. std::vector < std::pair < float, float >> angular_response_curve; + + /// Tier-2 backscatter correction (cube_bathymetry#87): when true, the loaded + /// angular_response_curve is a TL-REMOVED residual, so the estimator removes + /// the per-beam 2-way transmission loss `40*log10(R) + 2*alpha*R` (R = per-beam + /// slant range, m) BEFORE subtracting the residual curve, making the curve + /// depth/range transferable. Set by the curve loader from the CSV header + /// (`# tl_removed: `). Default false = tier-1 (no TL term), fully + /// backward compatible. Used only when backscatter_angle_correction == + /// Empirical. + bool backscatter_tl_removed = false; + + /// Tier-2 absorption coefficient alpha in dB/m (cube_bathymetry#87). The scalar + /// the estimator multiplies into the TL term `2*alpha*R`; read verbatim from + /// the curve CSV header (`# absorption_db_per_m: `) so the C++ estimator + /// NEVER recomputes the (Francois-Garrison) absorption -- the Python derive tool + /// is the single source of truth, guaranteeing Python/C++ consistency. Default + /// 0 = no absorption term (tier-1, or fresh water at negligible alpha). + float backscatter_absorption_db_per_m = 0.0f; }; } // namespace cube diff --git a/cube_bathymetry/include/cube_bathymetry/sounding.h b/cube_bathymetry/include/cube_bathymetry/sounding.h index 4cc5462..a369e85 100644 --- a/cube_bathymetry/include/cube_bathymetry/sounding.h +++ b/cube_bathymetry/include/cube_bathymetry/sounding.h @@ -41,6 +41,10 @@ namespace cube : depth(depth) { auto range = detections.two_way_travel_times[i] * detections.ping_info.sound_speed / 2.0; + // Retain the per-beam slant range as a first-class field (not only baked into + // sonar_relative_position) so the tier-2 backscatter TL correction can read it + // downstream (cube_bathymetry#87). Same value used for the geometry below. + slant_range = static_cast < float > (range); float tx_angle = 0.0; if(i < detections.tx_angles.size()) { tx_angle = detections.tx_angles[i]; @@ -118,6 +122,14 @@ namespace cube /// would bias the incidence correction and corrupt the settled backscatter. float beam_angle = std::nan(""); + /// Per-beam slant range R from the sonar head to the touchdown, in meters + /// (`two_way_travel_times[i] * sound_speed / 2`). Retained for the tier-2 + /// backscatter 2-way transmission-loss correction (cube_bathymetry#87), which + /// removes `40*log10(R) + 2*alpha*R` so the empirical angular-response curve + /// becomes depth/range transferable. NaN when constructed without detections + /// (the `explicit Sounding(float depth)` ctor); the TL term is then skipped. + float slant_range = std::nan(""); + // Position relative to the sonar head, in meters. // For a typical down looking sonar, x is along the heading, y is to starboard, and z is down. geometry_msgs::msg::Point sonar_relative_position; diff --git a/cube_bathymetry/scripts/derive_angular_response.py b/cube_bathymetry/scripts/derive_angular_response.py index b86cdca..c4cf354 100755 --- a/cube_bathymetry/scripts/derive_angular_response.py +++ b/cube_bathymetry/scripts/derive_angular_response.py @@ -20,7 +20,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -""" +r""" Derive an empirical backscatter angular-response curve from a bag. Reads a marine_acoustic_msgs/SonarDetections topic, bins each beam's @@ -34,8 +34,20 @@ bin is ~0 and off-nadir bins are <= 0; the estimator subtracts this column from each beam's raw dB. NaN/inf intensities and beams flagged bad are skipped. +Tier-2 (cube_bathymetry#87): with ``--remove-tl`` the per-beam 2-way +transmission loss ``TL = 40*log10(R) + 2*alpha*R`` (R = ``twtt*c/2`` per beam, +alpha = freshwater Francois-Garrison absorption from ``--water-temp-c`` and the +per-ping frequency) is compensated -- ADDED BACK -- for each beam BEFORE binning +(a distant return lost more energy, so it is boosted to recover range-independent +backscatter, TVG-style), so the curve becomes a TL-removed residual that is +depth/range transferable. The CSV columns +are unchanged; the header records ``tl_removed``/``absorption_db_per_m`` so the +C++ estimator applies the identical TL (and never recomputes alpha). + Usage: - derive_angular_response.py -t -o + derive_angular_response.py [ ...] -t -o + derive_angular_response.py ... -t -o \\ + --remove-tl --water-temp-c 24.0 """ import argparse @@ -44,16 +56,60 @@ def parse_args(argv): + """Parse CLI args (bag(s), topic, output, bin width, tier-2 TL options).""" p = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - p.add_argument('bag', help='rosbag2 directory (or .mcap/.db3 file)') + p.add_argument('bag', nargs='+', + help='one or more rosbag2 directories (or .mcap/.db3 files); ' + 'a real survey calibration spans many bags') p.add_argument('-t', '--topic', required=True, help='marine_acoustic_msgs/SonarDetections topic') p.add_argument('-o', '--output', required=True, help='output CSV path') p.add_argument('--bin-width-deg', type=float, default=2.0, help='angular bin width in degrees (default 2.0)') - return p.parse_args(argv) + p.add_argument('--remove-tl', action='store_true', + help='tier-2: remove per-beam 2-way transmission loss ' + '40*log10(R) + 2*alpha*R before binning (cube#87)') + p.add_argument('--water-temp-c', type=float, default=None, + help='water temperature, degC (REQUIRED with --remove-tl); ' + 'feeds the freshwater absorption alpha') + p.add_argument('--salinity', type=float, default=0.0, + help='salinity, psu (default 0.0 = fresh water). Non-zero ' + 'seawater absorption is out of scope for cube#87.') + p.add_argument('--depth-m', type=float, default=0.0, + help='nominal depth for the absorption pressure term D ' + '(default 0.0 -> P3=1 at lake depth)') + args = p.parse_args(argv) + if args.remove_tl and args.water_temp_c is None: + p.error('--water-temp-c is required when --remove-tl is set') + if args.remove_tl and args.salinity > 0.0: + p.error('--salinity > 0 (seawater) is out of scope for cube#87 ' + '(freshwater pure-water absorption only); omit --salinity') + return args + + +def freshwater_absorption_db_per_m(freq_khz, temp_c, depth_m=0.0): + """ + Francois-Garrison pure-water (freshwater) absorption alpha, dB/m. + + Only the pure-water term is needed in fresh water (the boric-acid and + MgSO4 seawater terms vanish at salinity 0): + + alpha_w(dB/km) = A3 * P3 * f_kHz**2 + + with the temperature-split A3 and the pressure term P3 (D in metres). + Returns alpha in dB/m (= alpha_w / 1000). Sanity: f=500 kHz, T=24 C, + D=0 -> ~0.049 dB/m. + """ + t = temp_c + if t > 20.0: + a3 = (3.964e-4 - 1.146e-5 * t + 1.45e-7 * t ** 2 - 6.5e-10 * t ** 3) + else: + a3 = (4.937e-4 - 2.590e-5 * t + 9.110e-7 * t ** 2 - 1.500e-8 * t ** 3) + p3 = 1.0 - 3.83e-5 * depth_m + 4.9e-10 * depth_m ** 2 + alpha_db_per_km = a3 * p3 * freq_khz ** 2 + return alpha_db_per_km / 1000.0 def open_reader(bag_path): @@ -73,8 +129,23 @@ def open_reader(bag_path): return reader -def accumulate(reader, topic, bin_width_deg): - """Return {bin_index: [sum_db, n]} for finite intensities on `topic`.""" +def accumulate(reader, topic, bin_width_deg, remove_tl, bins, freqs): + """ + Accumulate per-bin sums on `topic` into `bins` (merged across bags). + + Each bin slot is ``[sum_adjusted, sum_R, count]``: + + * tier-1 (``remove_tl`` False): ``sum_adjusted`` is the raw dB sum; ``sum_R`` + stays 0 (unused). + * tier-2 (``remove_tl`` True): the range-INDEPENDENT-of-alpha part of the TL + (the spreading term ``40*log10(R)``) is compensated (added back) inline, so + ``sum_adjusted`` is ``sum(db + 40*log10(R))`` and ``sum_R`` is ``sum(R)``. The + absorption term ``2*alpha*R`` is added in write_csv once alpha is known from the + median frequency -- a single pass over the bag. + + `freqs` collects each ping's ``ping_info.frequency`` (Hz) for the median. + Returns the number of messages read on `topic`. + """ from rclpy.serialization import deserialize_message from rosidl_runtime_py.utilities import get_message @@ -85,7 +156,6 @@ def accumulate(reader, topic, bin_width_deg): % (topic, ', '.join(sorted(type_map)) or 'none')) msg_type = get_message(type_map[topic]) - bins = {} # bin_index -> [sum_db, count] n_msgs = 0 while reader.has_next(): name, data, _stamp = reader.read_next() @@ -95,6 +165,11 @@ def accumulate(reader, topic, bin_width_deg): n_msgs += 1 intensities = msg.intensities rx_angles = msg.rx_angles + twtt = msg.two_way_travel_times + sound_speed = msg.ping_info.sound_speed + freq = msg.ping_info.frequency + if remove_tl and math.isfinite(freq) and freq > 0.0: + freqs.append(freq) # only needed for the tier-2 absorption alpha flags = getattr(msg, 'flags', None) count = min(len(intensities), len(rx_angles)) for i in range(count): @@ -109,29 +184,69 @@ def accumulate(reader, topic, bin_width_deg): ang = rx_angles[i] if not math.isfinite(ang): continue + + range_m = 0.0 + if remove_tl: + # R = twtt * c / 2; skip beams with a non-positive / NaN range + # (log10 undefined) so they never poison the residual. NOTE: this + # DROPS such beams from the derivation entirely, whereas the C++ + # estimator RETAINS them and just skips the TL term (tier-1 fallback + # for that beam) -- an intentional asymmetry: a beam with no valid + # range can't inform the TL-removed residual, but its raw value is + # still a usable backscatter sample at apply time. + if i >= len(twtt): + continue + range_m = twtt[i] * sound_speed / 2.0 + if not math.isfinite(range_m) or range_m <= 0.0: + continue + adjusted = db + 40.0 * math.log10(range_m) + else: + adjusted = db + abs_deg = abs(math.degrees(ang)) idx = int(abs_deg // bin_width_deg) - slot = bins.setdefault(idx, [0.0, 0]) - slot[0] += db - slot[1] += 1 - return bins, n_msgs + slot = bins.setdefault(idx, [0.0, 0.0, 0]) + slot[0] += adjusted + slot[1] += range_m + slot[2] += 1 + return n_msgs -def write_csv(bins, bin_width_deg, out_path): - """Write the binned means and db_relative_to_nadir to a curve CSV.""" +def write_csv(bins, bin_width_deg, out_path, remove_tl, alpha, water_temp_c): + """ + Write the binned means and db_relative_to_nadir to a curve CSV. + + For tier-2 (`remove_tl`), the absorption term ``2*alpha*R`` is added here + (compensated) using the per-bin mean range, completing the TL compensation + started in accumulate. The header records the TL provenance so the C++ + estimator applies the identical model. + """ if not bins: raise SystemExit('error: no finite intensity/angle samples found') - # Per-bin mean dB at the bin centre. + # Per-bin mean (TL-removed when tier-2) dB at the bin centre. rows = [] for idx in sorted(bins): - s, n = bins[idx] + s_adj, s_range, n = bins[idx] center = (idx + 0.5) * bin_width_deg - rows.append([center, s / n, n]) + mean = s_adj / n + if remove_tl: + # Compensate the absorption part of the TL: + 2*alpha*mean_R. + mean += 2.0 * alpha * (s_range / n) + rows.append([center, mean, n]) nadir_mean = rows[0][1] # lowest-angle (nadir-most) bin with open(out_path, 'w') as f: f.write('# Empirical angular response (derive_angular_response.py, cube#81)\n') + if remove_tl: + # Self-describing tier-2 header (cube#87): the estimator reads + # tl_removed + absorption_db_per_m and applies the identical TL. + f.write('# tl_removed: true\n') + f.write('# absorption_db_per_m: %.6g\n' % alpha) + f.write('# water_temp_c: %.3g\n' % water_temp_c) + f.write('# tl_model: 40*log10(R) + 2*alpha*R (R = twtt*c/2, metres)\n') + else: + f.write('# tl_removed: false\n') f.write('abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n') for center, mean_db, n in rows: db_rel = mean_db - nadir_mean @@ -139,14 +254,44 @@ def write_csv(bins, bin_width_deg, out_path): return len(rows) +def median(values): + """Plain median of a non-empty list (no numpy dependency).""" + s = sorted(values) + m = len(s) + mid = m // 2 + if m % 2 == 1: + return s[mid] + return 0.5 * (s[mid - 1] + s[mid]) + + def main(argv=None): - """Read the bag, bin by |rx_angles|, and write the curve CSV.""" + """Read the bag(s), bin by |rx_angles| and write the curve CSV (one line).""" args = parse_args(sys.argv[1:] if argv is None else argv) - reader = open_reader(args.bag) - bins, n_msgs = accumulate(reader, args.topic, args.bin_width_deg) - n_rows = write_csv(bins, args.bin_width_deg, args.output) - print('read %d pings on %s; wrote %d-bin curve to %s' - % (n_msgs, args.topic, n_rows, args.output)) + + bins = {} # bin_index -> [sum_adjusted, sum_R, count], merged across bags + freqs = [] # per-ping ping_info.frequency (Hz) + n_msgs = 0 + for bag_path in args.bag: + reader = open_reader(bag_path) + n_msgs += accumulate( + reader, args.topic, args.bin_width_deg, args.remove_tl, bins, freqs) + + alpha = 0.0 + if args.remove_tl: + if not freqs: + raise SystemExit( + 'error: --remove-tl needs a finite ping_info.frequency; none found') + freq_khz = median(freqs) / 1000.0 + alpha = freshwater_absorption_db_per_m( + freq_khz, args.water_temp_c, args.depth_m) + print('tier-2: median frequency %.1f kHz, T=%.1f C, D=%.1f m -> ' + 'alpha=%.6g dB/m' % (freq_khz, args.water_temp_c, args.depth_m, alpha)) + + n_rows = write_csv( + bins, args.bin_width_deg, args.output, + args.remove_tl, alpha, args.water_temp_c if args.remove_tl else 0.0) + print('read %d pings across %d bag(s) on %s; wrote %d-bin curve to %s' + % (n_msgs, len(args.bag), args.topic, n_rows, args.output)) return 0 diff --git a/cube_bathymetry/src/angular_response_curve.cpp b/cube_bathymetry/src/angular_response_curve.cpp index 0a12f5e..346fb79 100644 --- a/cube_bathymetry/src/angular_response_curve.cpp +++ b/cube_bathymetry/src/angular_response_curve.cpp @@ -52,23 +52,82 @@ bool parseBackscatterAngleCorrection( return false; } -std::vector> loadAngularResponseCurve(const std::string & path) +namespace +{ +// Parse a `# key: value` provenance comment. Returns true and fills `value` +// (trimmed) when `line` (already known to start with '#') matches `key`. +bool matchHeaderComment( + const std::string & line, const std::string & key, std::string & value) { - std::vector> curve; + // Strip the leading '#' and surrounding whitespace, then split on the first + // ':'. Tolerant of arbitrary leading whitespace / spacing around the colon. + const auto hash = line.find('#'); + std::string body = line.substr(hash + 1); + const auto colon = body.find(':'); + if (colon == std::string::npos) { + return false; + } + std::string k = body.substr(0, colon); + std::string v = body.substr(colon + 1); + auto trim = [](std::string & s) { + const auto b = s.find_first_not_of(" \t\r\n"); + const auto e = s.find_last_not_of(" \t\r\n"); + if (b == std::string::npos) { + s.clear(); + } else { + s = s.substr(b, e - b + 1); + } + }; + trim(k); + trim(v); + if (k != key) { + return false; + } + value = v; + return true; +} +} // namespace + +AngularResponseCurve loadAngularResponseCurveWithHeader(const std::string & path) +{ + AngularResponseCurve result; + std::vector> & curve = result.points; if (path.empty()) { - return curve; + return result; } std::ifstream in(path); if (!in) { - return curve; + return result; } std::string line; while (std::getline(in, line)) { - // Skip blank lines and comments. + // Skip blank lines and comments -- but first mine comments for the optional + // self-describing TL header (cube_bathymetry#87). const auto first = line.find_first_not_of(" \t\r\n"); if (first == std::string::npos || line[first] == '#') { + if (first != std::string::npos) { + std::string value; + if (matchHeaderComment(line, "tl_removed", value)) { + // Case-insensitive "true"/"1" -> true; anything else -> false. + std::string lower; + for (char c : value) { + lower.push_back(static_cast(std::tolower(static_cast(c)))); + } + result.tl_removed = (lower == "true" || lower == "1"); + } else if (matchHeaderComment(line, "absorption_db_per_m", value)) { + try { + std::size_t used = 0; + const float a = std::stof(value, &used); + if (used > 0) { + result.absorption_db_per_m = a; + } + } catch (const std::exception &) { + // Malformed absorption value -- leave at the tier-1 default (0). + } + } + } continue; } @@ -113,7 +172,12 @@ std::vector> loadAngularResponseCurve(const std::string return a.first < b.first; }); - return curve; + return result; +} + +std::vector> loadAngularResponseCurve(const std::string & path) +{ + return loadAngularResponseCurveWithHeader(path).points; } } // namespace cube diff --git a/cube_bathymetry/src/cube_bathymetry_node.cpp b/cube_bathymetry/src/cube_bathymetry_node.cpp index 52af5aa..fb0b431 100644 --- a/cube_bathymetry/src/cube_bathymetry_node.cpp +++ b/cube_bathymetry/src/cube_bathymetry_node.cpp @@ -119,13 +119,13 @@ class CubeBathymetry : public rclcpp_lifecycle::LifecycleNode "defaulting to none (no correction).", bs_correction_str.c_str()); bs_mode = cube::BackscatterAngleCorrection::None; } - std::vector> bs_curve; + cube::AngularResponseCurve bs_curve; if (bs_mode == cube::BackscatterAngleCorrection::Empirical && !bs_curve_file.empty()) { - bs_curve = cube::loadAngularResponseCurve(bs_curve_file); + bs_curve = cube::loadAngularResponseCurveWithHeader(bs_curve_file); } - if (bs_mode == cube::BackscatterAngleCorrection::Empirical && bs_curve.empty()) { + if (bs_mode == cube::BackscatterAngleCorrection::Empirical && bs_curve.points.empty()) { // Loud, not silent: the operator asked for the correction but no curve was // loaded (empty/missing/unparseable file), so it degrades to a no-op. RCLCPP_WARN(get_logger(), @@ -134,11 +134,23 @@ class CubeBathymetry : public rclcpp_lifecycle::LifecycleNode "(intensity emitted uncorrected). Provide a valid curve CSV.", bs_curve_file.c_str()); } else if (bs_mode == cube::BackscatterAngleCorrection::Empirical) { - RCLCPP_INFO(get_logger(), - "Backscatter angular-response correction: empirical, %zu-point curve " - "from %s", bs_curve.size(), bs_curve_file.c_str()); + if (bs_curve.tl_removed) { + // tier-2 (cube#87): TL-removed residual; estimator also removes + // 40*log10(R) + 2*alpha*R per beam (R from the sensor-frame point norm). + RCLCPP_INFO(get_logger(), + "Backscatter angular-response correction: empirical, %zu-point curve " + "from %s [tier-2: TL-removed, alpha=%g dB/m]", bs_curve.points.size(), + bs_curve_file.c_str(), + static_cast(bs_curve.absorption_db_per_m)); + } else { + RCLCPP_INFO(get_logger(), + "Backscatter angular-response correction: empirical, %zu-point curve " + "from %s", bs_curve.points.size(), bs_curve_file.c_str()); + } } - geo_map_sheet_->setBackscatterCorrection(bs_mode, std::move(bs_curve)); + geo_map_sheet_->setBackscatterCorrection( + bs_mode, std::move(bs_curve.points), + bs_curve.tl_removed, bs_curve.absorption_db_per_m); // Long-duration bounding (#70, ADR-0001). Declared BEFORE the draft prime so // the prime can be trimmed to the same budget -- otherwise loadIntoSheet @@ -957,6 +969,15 @@ class CubeBathymetry : public rclcpp_lifecycle::LifecycleNode s.sounding.horizontal_error = hu; s.sounding.intensity = intensity; // per-beam backscatter (may be NaN) s.sounding.beam_angle = beam_angle; // incidence rel. nadir (may be NaN) + // Per-beam slant range for the tier-2 TL correction (cube#87). The + // /soundings cloud carries the touchdown in the SENSOR frame, so its norm + // equals the slant range R = twtt*c/2 EXACTLY when the transmit tilt is + // zero -- which holds for the M3 (flat downward array, tx_angle == 0, the + // only tier-2-calibrated sonar today). For a tilted-transmit sonar the + // norm is R*sqrt(1 + sin^2(tx)*sin^2(rx)), a small bounded overestimate; + // the offline import_bag path uses twtt*c/2 directly, so live/offline + // match for the M3. Revisit (carry twtt) if a tilted-tx sonar is added. + s.sounding.slant_range = std::sqrt(x * x + y * y + z * z); soundings.push_back(s); } diff --git a/cube_bathymetry/src/geo_map_sheet.cpp b/cube_bathymetry/src/geo_map_sheet.cpp index b3f8143..a6554ef 100644 --- a/cube_bathymetry/src/geo_map_sheet.cpp +++ b/cube_bathymetry/src/geo_map_sheet.cpp @@ -37,11 +37,15 @@ GeoMapSheet::GeoMapSheet(float cell_size, std::string iho_order) void GeoMapSheet::setBackscatterCorrection( BackscatterAngleCorrection mode, - std::vector> curve) + std::vector> curve, + bool tl_removed, + float absorption_db_per_m) { // Grids hold a const reference to parameters_, so this reaches all of them. parameters_.backscatter_angle_correction = mode; parameters_.angular_response_curve = std::move(curve); + parameters_.backscatter_tl_removed = tl_removed; + parameters_.backscatter_absorption_db_per_m = absorption_db_per_m; } void GeoMapSheet::addSoundings( diff --git a/cube_bathymetry/src/hypothesis.cpp b/cube_bathymetry/src/hypothesis.cpp index 25a99e3..722935a 100644 --- a/cube_bathymetry/src/hypothesis.cpp +++ b/cube_bathymetry/src/hypothesis.cpp @@ -115,16 +115,18 @@ bool Hypothesis::update(float depth, float variance, const Parameters & paramete return true; } -void Hypothesis::recordBeam(float raw_intensity, float beam_angle) +void Hypothesis::recordBeam(float raw_intensity, float beam_angle, float range) { // Skip beams with no reported intensity -- a NaN must never be mistaken for a // real backscatter sample (ADR-0007: missing intensity != zero backscatter). // A NaN beam_angle is allowed through: the beam is still a valid intensity - // measurement, it merely cannot be angle-corrected at output. + // measurement, it merely cannot be angle-corrected at output. A NaN / non- + // positive range is likewise allowed through (the tier-2 TL term is skipped + // for that beam, cube_bathymetry#87). if (std::isnan(raw_intensity)) { return; } - intensity_samples.push_back(BeamIntensitySample{raw_intensity, beam_angle}); + intensity_samples.push_back(BeamIntensitySample{raw_intensity, beam_angle, range}); } diff --git a/cube_bathymetry/src/import_bag_main.cpp b/cube_bathymetry/src/import_bag_main.cpp index 3d865b8..e07b29f 100644 --- a/cube_bathymetry/src/import_bag_main.cpp +++ b/cube_bathymetry/src/import_bag_main.cpp @@ -90,7 +90,9 @@ "per-sonar curve from --backscatter-curve (cube#81)\n"; std::cout << " --backscatter-curve : empirical angular-response curve CSV " "(abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir). Required for " - "--backscatter-correction empirical; empty -> correction is a no-op\n"; + "--backscatter-correction empirical; empty -> correction is a no-op. A tier-2 " + "curve (header '# tl_removed: true' + '# absorption_db_per_m: ') also makes " + "the estimator remove per-beam 2-way TL 40*log10(R)+2*alpha*R (cube#87)\n"; std::cout << " -l : Stop after this many pings (debugging)\n"; std::cout << " --source-id : Registry source id recorded for every cell " "(default cube-replay)\n"; @@ -457,14 +459,14 @@ int main(int argc, char * argv[]) << "(got '" << backscatter_correction_str << "')\n"; usage(); } - std::vector> backscatter_curve; + cube::AngularResponseCurve backscatter_curve; if (backscatter_mode == cube::BackscatterAngleCorrection::Empirical && !backscatter_curve_file.empty()) { - backscatter_curve = cube::loadAngularResponseCurve(backscatter_curve_file); + backscatter_curve = cube::loadAngularResponseCurveWithHeader(backscatter_curve_file); } if (backscatter_mode == cube::BackscatterAngleCorrection::Empirical && - backscatter_curve.empty()) + backscatter_curve.points.empty()) { // Loud, not silent: enabled but no curve loaded -> correction is a no-op. std::cerr << "warning: --backscatter-correction empirical but no curve was " @@ -473,11 +475,19 @@ int main(int argc, char * argv[]) "uncorrected). Provide a valid curve CSV.\n"; } else if (backscatter_mode == cube::BackscatterAngleCorrection::Empirical) { std::cout << "Backscatter angular-response correction: empirical, " - << backscatter_curve.size() << "-point curve from " - << backscatter_curve_file << std::endl; + << backscatter_curve.points.size() << "-point curve from " + << backscatter_curve_file; + if (backscatter_curve.tl_removed) { + // tier-2 (cube#87): the curve is a TL-removed residual; the estimator + // also removes 40*log10(R) + 2*alpha*R per beam. + std::cout << " [tier-2: TL-removed, alpha=" + << backscatter_curve.absorption_db_per_m << " dB/m]"; + } + std::cout << std::endl; } geo_map_sheet.setBackscatterCorrection( - backscatter_mode, std::move(backscatter_curve)); + backscatter_mode, std::move(backscatter_curve.points), + backscatter_curve.tl_removed, backscatter_curve.absorption_db_per_m); std::cout << "reading messages..." << std::endl; @@ -589,6 +599,9 @@ int main(int argc, char * argv[]) // emits the value UNCORRECTED; the angle correction is cube#81. gs.sounding.intensity = s.intensity; gs.sounding.beam_angle = s.beam_angle; + // Per-beam slant range R = twtt*c/2 (set in the Sounding detections + // ctor) for the tier-2 TL correction (cube#87). + gs.sounding.slant_range = s.slant_range; soundings.push_back(gs); } geo_map_sheet.addSoundings(soundings); diff --git a/cube_bathymetry/src/node.cpp b/cube_bathymetry/src/node.cpp index 420bd71..00296cf 100644 --- a/cube_bathymetry/src/node.cpp +++ b/cube_bathymetry/src/node.cpp @@ -116,7 +116,7 @@ void Node::seedSettledDepth( bool Node::update( float depth, float variance, const Parameters & parameters, - float intensity, float beam_angle) + float intensity, float beam_angle, float range) { /* Find the best matching hypothesis for the current input sample given * those currently being tracked. @@ -131,14 +131,14 @@ bool Node::update( addHypothesis(depth, variance); // This beam seeded (and so is a member of) the new hypothesis: record its // backscatter on it. Without this the first beam at every node is dropped. - depth_hypotheses_.back()->recordBeam(intensity, beam_angle); + depth_hypotheses_.back()->recordBeam(intensity, beam_angle, range); return true; } else { /* Update the best hypothesis with the current data */ if(best->update(depth, variance, parameters)) { // Accepted into the winning hypothesis -- accumulate its backscatter on // the SAME hypothesis (ADR-0007 D2: intensity rides the winning depth). - best->recordBeam(intensity, beam_angle); + best->recordBeam(intensity, beam_angle, range); } else { /* Failed update --- indicates an intervention, so that we need to * start a new hypothesis to capture the outlier/datum shift. @@ -148,7 +148,7 @@ bool Node::update( // The beam was REJECTED from `best` (depth-geometry outlier) and used to // seed the new hypothesis, so its backscatter belongs to the new // hypothesis, NOT to `best` (exclusion-on-intervention invariant). - depth_hypotheses_.back()->recordBeam(intensity, beam_angle); + depth_hypotheses_.back()->recordBeam(intensity, beam_angle, range); } } return true; @@ -253,17 +253,18 @@ bool Node::insert(double distance, const Sounding & sounding, const Parameters & nominated_hypothesis_.reset(); return queueEstimate(sounding.depth + offset, variance, parameters, - sounding.intensity, sounding.beam_angle); + sounding.intensity, sounding.beam_angle, sounding.slant_range); } bool Node::queueEstimate( float depth, float variance, const Parameters & parameters, - float intensity, float beam_angle) + float intensity, float beam_angle, float range) { if(queue_.size() >= parameters.median_length) { auto mi = queue_.begin(); advance(mi, parameters.median_length / 2); - update(mi->depth, mi->uncertainty, parameters, mi->intensity, mi->beam_angle); + update(mi->depth, mi->uncertainty, parameters, mi->intensity, mi->beam_angle, + mi->range); queue_.erase(mi); } @@ -271,7 +272,7 @@ bool Node::queueEstimate( while(i != queue_.end() && i->depth > depth) { i++; } - queue_.insert(i, DepthAndUncertainty(depth, variance, intensity, beam_angle)); + queue_.insert(i, DepthAndUncertainty(depth, variance, intensity, beam_angle, range)); if(queue_.size() >= parameters.median_length) { /* Compute the likely 99% confidence bound below the shallowest point, and @@ -358,21 +359,48 @@ NodeRecord Node::extractNodeRecord(const Parameters & parameters) // intensity in dB). The curve is keyed on |beam_angle|, so port/starboard beams // of equal magnitude get the same correction. // + // Tier-2 (cube_bathymetry#87): when the loaded curve is a TL-REMOVED residual + // (backscatter_tl_removed), compensate each beam's 2-way transmission loss by + // ADDING IT BACK -- a distant return lost more energy, so it is boosted to + // recover range-independent backscatter (TVG-style): + // TL(R) = 40*log10(R) + 2*alpha*R (R = per-beam slant range, m) + // so the residual curve is depth/range transferable: + // corrected = raw + TL(R) - residualCurve(|beam_angle|). + // alpha is read verbatim from the curve header (backscatter_absorption_db_per_m); + // the estimator NEVER recomputes the (Francois-Garrison) absorption -- the + // Python derive tool is the single source of truth, guaranteeing consistency. + // A NaN / non-positive R skips the TL term (no log of a non-positive range); + // tier-1 (backscatter_tl_removed == false) skips it entirely (unchanged). + // // Deferred (NOT done here): the full radiometric GeoCoder chain -- insonified // area, beam-pattern, TVG residual, and the depth/slope incidence term // (ADR-0007 D3, cube_bathymetry#15/#59). The empirical curve absorbs the // aggregate angular falloff for a flat bottom; per-beam {raw_intensity, - // beam_angle} stay retained so the node value is re-derivable when #15 lands. + // beam_angle, range} stay retained so the node value is re-derivable when #15 + // lands. const bool apply_ara = parameters.backscatter_angle_correction == BackscatterAngleCorrection::Empirical && !parameters.angular_response_curve.empty(); + const bool apply_tl = apply_ara && parameters.backscatter_tl_removed; + const double alpha = parameters.backscatter_absorption_db_per_m; double sum = 0.0; double sum_sq = 0.0; uint32_t n = 0; for(const auto & sample : chosen->intensity_samples) { // recordBeam() already excluded NaN-intensity beams, so raw_intensity is real. double corrected = sample.raw_intensity; + if(apply_tl) { + const double range = static_cast(sample.range); + // Skip the TL term for a missing / non-positive range (log10 undefined); + // the beam is still corrected by the residual angular-response curve below. + if(std::isfinite(range) && range > 0.0) { + // Compensate (ADD BACK) the 2-way transmission loss: a distant return + // lost more energy, so boost it to recover range-independent backscatter + // (TVG-style). #87 sign fix. + corrected += 40.0 * std::log10(range) + 2.0 * alpha * range; + } + } if(apply_ara && !std::isnan(sample.beam_angle)) { const double abs_angle_deg = std::abs(static_cast(sample.beam_angle)) * 180.0 / M_PI; @@ -496,7 +524,7 @@ void Node::queueFlush(const Parameters & parameters) while (ex_pt >= 0) { update(q[ex_pt].depth, q[ex_pt].uncertainty, parameters, - q[ex_pt].intensity, q[ex_pt].beam_angle); + q[ex_pt].intensity, q[ex_pt].beam_angle, q[ex_pt].range); ex_pt += direction * scale; direction = -direction; scale++; diff --git a/cube_bathymetry/test/test_angular_response_curve.cpp b/cube_bathymetry/test/test_angular_response_curve.cpp index 290fc17..45cba04 100644 --- a/cube_bathymetry/test/test_angular_response_curve.cpp +++ b/cube_bathymetry/test/test_angular_response_curve.cpp @@ -92,6 +92,85 @@ TEST(AngularResponseCurve, MissingFileIsEmpty) EXPECT_TRUE(loadAngularResponseCurve("/no/such/curve_file_12345.csv").empty()); } +// Tier-2 (#87): a curve with the self-describing TL header parses tl_removed + +// absorption_db_per_m; the data columns are unchanged. +TEST(AngularResponseCurve, ParsesTier2Header) +{ + const std::string csv = + "# Empirical angular response (tier-2)\n" + "# tl_removed: true\n" + "# absorption_db_per_m: 0.04897\n" + "# water_temp_c: 24\n" + "# tl_model: 40*log10(R) + 2*alpha*R (R = twtt*c/2, metres)\n" + "abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n" + "1.0,-40.0,100,+0.000\n" + "3.0,-41.0,100,-1.000\n"; + const std::string path = writeTemp(csv); + + auto loaded = loadAngularResponseCurveWithHeader(path); + std::remove(path.c_str()); + + EXPECT_TRUE(loaded.tl_removed); + EXPECT_FLOAT_EQ(loaded.absorption_db_per_m, 0.04897f); + ASSERT_EQ(loaded.points.size(), 2u); + EXPECT_FLOAT_EQ(loaded.points[0].first, 1.0f); + EXPECT_FLOAT_EQ(loaded.points[1].second, -1.0f); +} + +// Absence of the TL header -> tier-1 defaults (tl_removed=false, absorption=0), +// so a tier-1 curve keeps loading unchanged. +TEST(AngularResponseCurve, AbsentHeaderDefaultsToTier1) +{ + const std::string csv = + "# M3 empirical angular response\n" + "abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n" + "1.0,-40.0,100,+0.000\n"; + const std::string path = writeTemp(csv); + + auto loaded = loadAngularResponseCurveWithHeader(path); + std::remove(path.c_str()); + + EXPECT_FALSE(loaded.tl_removed); + EXPECT_FLOAT_EQ(loaded.absorption_db_per_m, 0.0f); + ASSERT_EQ(loaded.points.size(), 1u); +} + +// An explicit "# tl_removed: false" header parses as tier-1 (the derive tool +// writes this for tier-1 runs). +TEST(AngularResponseCurve, ExplicitTlRemovedFalse) +{ + const std::string csv = + "# tl_removed: false\n" + "abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n" + "1.0,-40.0,100,+0.000\n"; + const std::string path = writeTemp(csv); + + auto loaded = loadAngularResponseCurveWithHeader(path); + std::remove(path.c_str()); + + EXPECT_FALSE(loaded.tl_removed); + EXPECT_FLOAT_EQ(loaded.absorption_db_per_m, 0.0f); +} + +// The backward-compatible loadAngularResponseCurve() still returns just the +// points (drops the TL provenance) -- existing call sites are unaffected. +TEST(AngularResponseCurve, BackCompatLoaderReturnsPoints) +{ + const std::string csv = + "# tl_removed: true\n" + "# absorption_db_per_m: 0.05\n" + "abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n" + "1.0,-40.0,100,+0.000\n" + "3.0,-41.0,100,-1.000\n"; + const std::string path = writeTemp(csv); + + auto curve = loadAngularResponseCurve(path); + std::remove(path.c_str()); + + ASSERT_EQ(curve.size(), 2u); + EXPECT_FLOAT_EQ(curve[1].second, -1.0f); +} + // Mode-string parsing is case-insensitive; unknown values are rejected. TEST(AngularResponseCurve, ParsesMode) { diff --git a/cube_bathymetry/test/test_node.cpp b/cube_bathymetry/test/test_node.cpp index 56fea50..6d61fe6 100644 --- a/cube_bathymetry/test/test_node.cpp +++ b/cube_bathymetry/test/test_node.cpp @@ -684,4 +684,130 @@ TEST_F(NodeTest, ARAPortStarboardSymmetry) EXPECT_NEAR(record.intensity_var, 0.0f, 1e-4); } +// ---- Tier-2: TL-removed angular response (#87) ----------------------------- + +namespace +{ +// empiricalParams() + the tier-2 TL provenance: the loaded curve is a TL-removed +// residual, so the estimator also compensates (ADDS BACK) 40*log10(R) + 2*alpha*R +// per beam. +Parameters tier2Params(float alpha) +{ + Parameters p = empiricalParams(); // Empirical, curve {{0,0},{60,-12}} + p.backscatter_tl_removed = true; + p.backscatter_absorption_db_per_m = alpha; + return p; +} + +// Single-beam nominated hypothesis carrying a per-beam slant range, so the +// surfaced intensity equals the tier-2 corrected value of exactly that beam. +std::shared_ptr singleBeamNominatedRange( + Node & n, float raw_intensity, float beam_angle_rad, float range) +{ + auto h = std::make_shared(10.0f, 1.0f); + h->input_sample_variance = 1.0f; + h->recordBeam(raw_intensity, beam_angle_rad, range); + NodeNominationTestAccess::nominate(n, h); + return h; +} + +// Reference tier-2 correction: corrected = raw + (40log10R + 2*alpha*R) - residual. +// TL is ADDED BACK (compensated): a distant return lost more energy, so it is +// boosted to recover range-independent backscatter (TVG-style, #87 sign fix). +double tier2Expected(double raw, double range, double alpha, double residual_db) +{ + const double tl = 40.0 * std::log10(range) + 2.0 * alpha * range; + return raw + tl - residual_db; +} +} // namespace + +// Tier-2 mid-angle beam with a known range: the TL term is compensated and the +// residual curve removed -- corrected = raw + (40log10R + 2*alpha*R) - residual(30deg). +TEST_F(NodeTest, Tier2RemovesTLAndResidual) +{ + const float alpha = 0.05f; + const float range = 50.0f; + Parameters t2 = tier2Params(alpha); + Node n; + singleBeamNominatedRange(n, -30.0f, deg2rad(30.0f), range); + + auto record = n.extractNodeRecord(t2); + ASSERT_EQ(record.n_samples, 1u); + // residual(30deg) = -6 dB (interpolated {0,0}..{60,-12}). + const double expected = tier2Expected(-30.0, range, alpha, -6.0); + EXPECT_NEAR(record.intensity, static_cast(expected), 1e-2); +} + +// Tier-2 nadir beam: residual(0) = 0, only the TL term is removed. +TEST_F(NodeTest, Tier2NadirRemovesTLOnly) +{ + const float alpha = 0.04897f; + const float range = 35.0f; + Parameters t2 = tier2Params(alpha); + Node n; + singleBeamNominatedRange(n, -40.0f, 0.0f, range); + + auto record = n.extractNodeRecord(t2); + ASSERT_EQ(record.n_samples, 1u); + const double expected = tier2Expected(-40.0, range, alpha, 0.0); + EXPECT_NEAR(record.intensity, static_cast(expected), 1e-2); +} + +// Tier-2 with a NaN range: the TL term is skipped (no log of a non-positive R), +// so the beam falls back to tier-1 (residual-only) correction. +TEST_F(NodeTest, Tier2NaNRangeSkipsTL) +{ + Parameters t2 = tier2Params(0.05f); + Node n; + singleBeamNominatedRange(n, -30.0f, deg2rad(30.0f), std::nan("")); + + auto record = n.extractNodeRecord(t2); + ASSERT_EQ(record.n_samples, 1u); + // No TL -> corrected = raw - residual(30deg) = -30 - (-6) = -24. + EXPECT_NEAR(record.intensity, -24.0f, 1e-3); +} + +// Tier-2 with a non-positive range: TL skipped likewise (identity TL term). +TEST_F(NodeTest, Tier2NonPositiveRangeSkipsTL) +{ + Parameters t2 = tier2Params(0.05f); + Node n; + singleBeamNominatedRange(n, -30.0f, deg2rad(30.0f), -5.0f); + + auto record = n.extractNodeRecord(t2); + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -24.0f, 1e-3); +} + +// Regression: tl_removed == false (tier-1) ignores the range entirely, even when +// a finite range is present -- the TL term must NOT be applied. +TEST_F(NodeTest, Tier1IgnoresRange) +{ + Parameters ara = empiricalParams(); // tl_removed defaults false + Node n; + singleBeamNominatedRange(n, -30.0f, deg2rad(30.0f), 50.0f); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 1u); + // Tier-1: corrected = raw - residual(30deg) = -24 (no TL despite range=50). + EXPECT_NEAR(record.intensity, -24.0f, 1e-3); +} + +// Threading: a per-beam range survives Node::update() onto the seeding/winning +// hypothesis, so the tier-2 TL correction sees it at extractNodeRecord(). +TEST_F(NodeTest, Tier2RangeThreadsThroughUpdate) +{ + const float alpha = 0.05f; + const float range = 50.0f; + Parameters t2 = tier2Params(alpha); + Node n; + // !best path (first beam seeds a new hypothesis) carries the range. + ASSERT_TRUE(n.update(10.0f, 1.0f, t2, -30.0f, deg2rad(30.0f), range)); + + auto record = n.extractNodeRecord(t2); + ASSERT_EQ(record.n_samples, 1u); + const double expected = tier2Expected(-30.0, range, alpha, -6.0); + EXPECT_NEAR(record.intensity, static_cast(expected), 1e-2); +} + } // namespace cube