diff --git a/.agent/work-plans/issue-81/plan.md b/.agent/work-plans/issue-81/plan.md new file mode 100644 index 0000000..d6b084a --- /dev/null +++ b/.agent/work-plans/issue-81/plan.md @@ -0,0 +1,199 @@ +# Plan: Apply deferred M3 backscatter angle-correction in shared CUBE estimator + +## Issue + +https://github.com/rolker/cube_bathymetry/issues/81 + +## Context + +**Prior plan (cos²θ Lambert) SUPERSEDED.** Empirical analysis of `m3_dryrun` +`/detections` (2025 pings, 445k beams) showed cos²θ is the wrong model — it +explains only ~¼ of the observed nadir-to-edge falloff (~24 dB). The remaining +falloff is dominated by sonar-equation terms cos²θ ignores: insonified area, +beam-pattern, and TVG residual. The correct approach is an **empirical +angular-response (ARA/AVG) correction** driven by a per-sonar curve derived +offline from real data. + +**Current state.** `Node::extractNodeRecord()` (`node.cpp:315-318`) contains a +Phase B no-op: `corrected == raw` for every beam. Per-beam +`{raw_intensity, beam_angle}` pairs are already stored on each hypothesis +(`Hypothesis::intensity_samples`). + +**Design: configured curve + offline derivation tool, sonar-agnostic, default +OFF.** The estimator is sonar-agnostic (M3, Norbit, R2Sonic, sim, …) — no +correction model is baked in. An operator derives a curve for their sonar offline +and configures the path at startup; default OFF preserves the current Phase B +identity behavior on all existing deployments. + +**Gate (already verified, document it).** `rx_angles` sign/zero confirmed against +`kongsberg_em_bridge`: Kongsberg pointing +PORT → negated → +STARBOARD; nadir=0; +radians; intensity in dB. Document as inline comment at the correction site. + +**Rename needed.** `BeamIntensitySample::grazing_angle` (hypothesis.h:49) is +misnamed — it stores the receive steering / incidence angle from nadir (same as +`sounding.beam_angle`), NOT a true grazing angle (90°−θ). The field doc already +says "receive/steering (beam/incidence) angle" — the name is wrong, the comment is +right. Plan renames it to `beam_angle`. + +## Approach + +0. **Rename `BeamIntensitySample::grazing_angle` → `beam_angle`** in: + - `hypothesis.h:49` (struct field decl) + - `hypothesis.h:115-116` (doc text "a NaN grazing_angle is retained") + - `hypothesis.h:166` (BeamIntensitySample reference in the memory-budget comment) + - `recordBeam` declaration (`hypothesis.h:118`) and implementation + (`hypothesis.cpp:118, 122, 127`) — parameter name + - `node.cpp` call-site comment + - `test_hypothesis.cpp:216, 218, 243` — `grazing_angle` field refs + +1. **Add `BackscatterAngleCorrection` to `Parameters` (`parameters.h`)**: + ``` + enum class BackscatterAngleCorrection { None, Empirical }; + BackscatterAngleCorrection backscatter_angle_correction = + BackscatterAngleCorrection::None; + std::vector> angular_response_curve; // {abs_angle_deg, db_relative_to_nadir} + ``` + The curve vector is populated at startup from the curve file; empty → correction + is a no-op even if mode is Empirical (log a warning). No new dependencies needed + (standard library only). + +2. **Plumb the parameter on both estimator entry points:** + - **Live node** (`cube_bathymetry_node.cpp`): declare ROS params + `backscatter_angle_correction` (string `"none"|"empirical"`, default `"none"`) + and `backscatter_curve_file` (string path, default `""`). At `on_configure()`, + parse the mode enum, load the curve file (CSV) into + `Parameters::angular_response_curve` if mode is Empirical and path is non-empty. + - **Offline import** (`import_bag_main.cpp`): add `--backscatter-correction + none|empirical` (default `none`) and `--backscatter-curve ` (default + empty) to `usage()` and arg parsing. Load the curve the same way at startup. + Both run the same estimator → one mechanism corrects both live (#78) and offline + (#80) paths. + +3. **Replace the Phase B no-op in `node.cpp:315-318`** with the ARA correction: + ```cpp + // ARA correction: corrected_dB = raw_dB − curveRel(|beam_angle|), where + // curveRel is the empirical angular-response curve's db_relative_to_nadir + // column, linearly interpolated between bin centres. Nadir → curveRel≈0 → + // identity. Beyond the curve's max angle OR NaN beam_angle → identity. + // rx_angles sign/zero: producer (kongsberg_em_bridge) negates Kongsberg + // pointing angle (+PORT→+STARBOARD; nadir=0; radians; intensity dB). + ``` + Logic: if `mode == Empirical && !isnan(beam_angle) && curve not empty`: + - `theta = std::abs(beam_angle)` converted to degrees + - If `theta > curve.back().first` → identity (beyond curve extent) + - Else linearly interpolate `curveRel` from adjacent bin centres + - `corrected = raw − curveRel` + - Else: `corrected = raw` (None mode, NaN angle, or empty curve) + Update the stale `TODO(#54-B / #15)` comment block (`node.cpp:298-312`): Phase B + flat-bottom done via ARA; retain note for the full radiometric GeoCoder (#15/#59). + +4. **Offline derivation tool** — Python script + `cube_bathymetry/scripts/derive_angular_response.py`: + - Reads a bag's `SonarDetections` topic + - Bins `intensities` by `|rx_angles|` (converted to degrees, 2° bins) + - Computes mean backscatter per bin; computes `db_relative_to_nadir` (0°-bin + mean − per-bin mean; nadir bin ≈ 0, off-nadir bins ≤ 0) + - Writes CSV with columns: `abs_angle_deg_center, mean_bs_db, n, + db_relative_to_nadir` (matches the host-written seed at + `~/data/logs/analysis/m3_angular_response_curve.csv`) + - CLI: `derive_angular_response.py -t -o ` + - Install via `ament_python_install_package` or list in `CMakeLists.txt` + `install(PROGRAMS ...)` + +5. **Tests** (`test_node.cpp`): + - **Default None path**: confirm existing intensity assertions (`FirstBeamInitializationRecordsIntensity`, + `NodeRecordMeanAndEstimateVariance`, `NodeRecordSkipsNanIntensityBeam`) remain + unchanged — they do NOT opt into the correction, so `corrected == raw` on the + default path. No assertion changes needed. + - **Empirical-ON tests** (pass a Parameters with mode=Empirical and a known + 2-point curve): + - `ARAInterpolation`: 2-point curve at 0° and 60° → mid-angle (30°) exact + interpolated subtract; `corrected ≠ raw`. + - `ARANadirIdentity`: θ≈0 (nadir bin centre) → curveRel≈0 → `corrected ≈ raw`. + - `ARABeyondMaxAngleIdentity`: θ > curve max → identity. + - `ARANaNAngleIdentity`: NaN beam_angle → identity. + - `ARAOffIsIdentity`: mode=None, non-nadir beam → `corrected == raw`. + - `ARAPortStarboardSymmetry`: ±θ same magnitude → same corrected value + (curve keyed on `|angle|`). + +6. **`test_store_import.cpp` (#80)**: default None means `BackscatterCellsMatchGridRecords` + stays valid (no correction applied). Update the stale comment at lines 64-67 + ("the surfaced value is uncorrected so the angle does not change it here") to + say "by default (BackscatterAngleCorrection::None)". + +7. **ADR-0007 transition addendum** — + `docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md` + (sibling addendum name; does **not** claim the `0007-` canonical slot before + the full ADR exists). Records: Phase B end, the ARA correction and offline tool, + the configured-curve + default-off rationale, what remains deferred (full + radiometric GeoCoder: area + beam-pattern + TVG + slope; ADR-0007 D3 / #15 / #59). + **Note in the addendum**: file a follow-up to author the full ADR-0007 document + and a follow-up for intensity-domain (dB vs linear) as a first-class sonar + property. + +## Files to Change + +| File | Change | +|------|--------| +| `include/cube_bathymetry/hypothesis.h` | Rename `grazing_angle` → `beam_angle` (struct field + doc at :115-116, :166 + `recordBeam` decl at :118) | +| `src/hypothesis.cpp` | Rename `grazing_angle` param in `recordBeam` impl (:118, :122, :127) | +| `include/cube_bathymetry/parameters.h` | Add `BackscatterAngleCorrection` enum + field (default None) + `angular_response_curve` vector | +| `src/node.cpp` | ARA correction with gate comment; update TODO block | +| `src/cube_bathymetry_node.cpp` | Declare/parse `backscatter_angle_correction` + `backscatter_curve_file` ROS params; load curve | +| `src/import_bag_main.cpp` | `--backscatter-correction` + `--backscatter-curve` flags + `usage()` text; load curve | +| `scripts/derive_angular_response.py` | New: offline curve-derivation tool | +| `CMakeLists.txt` | Install `scripts/derive_angular_response.py` | +| `test/test_node.cpp` | 6 ARA/param tests; verify 3 existing assertions unchanged on default path | +| `test/test_hypothesis.cpp` | `grazing_angle` → `beam_angle` refs (:216, :218, :243) | +| `test/test_store_import.cpp` | Update stale comment at :64-67 | +| `docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md` | New: addendum | + +## Principles Self-Check + +| Principle | Consideration | +|---|---| +| Human control and transparency | Default OFF; operator must explicitly enable and supply a curve file — no silent correction. | +| Enforcement over documentation | 6 targeted tests catch wrong interpolation, missing NaN/beyond-max guards, and the default-off path; port/starboard symmetry. | +| Only what's needed | ARA only; full radiometric GeoCoder (area + beam-pattern + TVG + slope) is explicit follow-up. Offline tool and correction plumbing together in one PR (tight scope). | +| A change includes its consequences | Parameters plumbed through both estimator entry points; rename covers all refs including header docs and tests; stale comment updated; addendum captures the decision. | +| Capture decisions | ADR-0007 transition addendum records the ARA design, offline-tool format, default-off rationale, and deferrals. | +| Improve incrementally | Phase B no-op → ARA (here) → full GeoCoder (#15/#59). Small, reviewable PR. | +| Test what breaks | 3 existing tests verified unchanged on default path; 6 new tests on Empirical path; rename covered by test_hypothesis.cpp update. | + +## ADR Compliance + +| ADR | Triggered | How addressed | +|---|---|---| +| ADR-0007 D3 (incidence correction at node-output) | Yes — primary | ARA correction implemented (param-gated) at `extractNodeRecord()` | +| ADR-0007 D2/D4 (per-beam mean + estimate variance) | Yes — already in place | `corrected` replaces `raw` in the sum; combine unchanged | +| ADR-0008 (ROS 2 conventions) | Yes | New node params declared with defaults; offline CLI flags documented in `usage()` | +| ADR-0001 (adopt ADRs) | Yes | Transition addendum in `docs/decisions/`; follow-up to author full ADR-0007 | + +## Consequences + +| If we change... | Also update... | Included? | +|---|---|---| +| `grazing_angle` → `beam_angle` rename | hypothesis.cpp, test_hypothesis.cpp, node.cpp comment, header docs :115-116, :166 | Yes — Step 0 | +| Add `BackscatterAngleCorrection` to Parameters | node ROS params + import_bag CLI + parameters.h | Yes — Steps 1-2 | +| ARA correction at node-output | test_node.cpp (6 new tests; 3 existing verified unchanged) | Yes — Step 5 | +| Default None preserves current behavior | test_store_import.cpp stale comment fixed | Yes — Step 6 | +| Phase B ends | ADR-0007 transition addendum + follow-up issues noted | Yes — Step 7 | +| Full radiometric GeoCoder | #15, #59; ADR-0007 full doc | No — explicit follow-up | +| Intensity domain (dB vs linear) as sonar property | Separate issue | No — noted in addendum | + +## Open Questions + +- [ ] **Script install method**: add `derive_angular_response.py` via + `install(PROGRAMS ...)` in CMakeLists.txt or via `ament_python_install_package`? + Prefer `install(PROGRAMS ...)` (single-file, no Python package structure needed) + — confirm if the team has a house standard. +- [ ] **Curve file format**: CSV was chosen (matches host-written seed). Confirm + whether YAML would be preferred for consistency with other ROS parameter files. + CSV is simpler for the offline tool to write; either is trivially parseable. + +## Estimated Scope + +Single PR, one package. ~180 lines changed (plumbing + correction logic + rename) ++ ~120 new test lines + ~80 lines for the Python script + ADR addendum. +Rename (`grazing_angle` → `beam_angle`) is a clean orthogonal split candidate if +the PR grows unwieldy, but it fits the scope as written. diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md new file mode 100644 index 0000000..ccd4420 --- /dev/null +++ b/.agent/work-plans/issue-81/progress.md @@ -0,0 +1,254 @@ +--- +issue: 81 +--- + +# Issue #81 — Apply deferred M3 backscatter angle-correction in shared CUBE estimator + +## Issue Review +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Code Agent (Claude Sonnet) + +**Issue**: #81 +**Comment**: (best-effort post follows this entry; not recorded inline) +**Scope verdict**: well-scoped + +### Summary + +Issue #81 proposes lifting the Phase B no-op at `node.cpp:298-322` (the +`TODO(#54-B / #15)` block) by applying a first-cut Lambert beam-angle-vs-nadir +correction per beam before combining into the settled intensity estimate. The +gate — verifying `rx_angles[i]` sign/zero convention — is resolvable from +existing sources and the full GeoCoder correction is explicitly deferred. + +### Scope Assessment + +**Well-scoped?** Yes — two sequential sub-tasks with a hard gate between them: +(1) verify sign convention, then (2) apply first-cut Lambert correction in +`node.cpp:298-322`. The full GeoCoder correction (#15) is explicitly a +follow-up. Both sub-tasks affect exactly one site in the shared estimator. + +**Right repo?** Yes — `cube_bathymetry` is the correct placement per the +accepted backscatter design (ADR-0007 D9): CUBE-coupled processing belongs in +this package, and the shared `Node::extractNodeRecord()` path is already the +correct single-dispatch point. + +**Dependencies:** +- #54 (per-beam `{intensity, beam_angle}` stats) — already merged; the + necessary `Hypothesis::intensity_samples` container and `beam_angle` field on + `Sounding` are in place. +- #78 (live tile transport) and #80 (offline store layer) — consumers, not + blockers. Correction landing here automatically propagates to both. +- #15 (slope correction / predicted-surface producer) — needed only for the + full GeoCoder correction (follow-up); this issue's first-cut Lambert does + not depend on it. +- #59 (predicted-surface producer) — inert dependency; still out of scope here. + +### Sign-Convention Verification (Gate Assessment) + +The gate is resolvable from available sources. `SonarDetections.msg` (v2.1.0, +`/opt/ros/jazzy/share/marine_acoustic_msgs/msg/SonarDetections.msg`) explicitly +states: + +``` +# Sonar-reported across-track steering angle (applied to rx beam) +# + to starboard, - to port for a downlooking sonar +# reported in radians +float32[] rx_angles +``` + +`sounding.h:109` already documents `beam_angle` as "positive to starboard (the +detections.rx_angles convention)" — consistent with the producer. The position +computation at `sounding.h:49` (`sonar_relative_position.y = range * +sin(detections.rx_angles[i])`) confirms the convention is correctly applied. + +**Sign-convention conclusion:** The producer and the stored `beam_angle` field +agree. The gate can be passed by documenting this cross-check in the commit or +as an inline comment at the correction site. No discrepancy found. + +**Watch:** For the Lambert correction `corrected = raw - f(angle)`, the +incidence angle from vertical is `|beam_angle|` (always non-negative; nadir = +0). The implementation must use `std::abs(sample.beam_angle)` rather than the +raw signed value, or the correction will have an asymmetric sign error across +port/starboard beams. This is the key correctness invariant to enforce in +implementation and test. + +### Principle Alignment + +| Principle | Status | Notes | +|---|---|---| +| Human control and transparency | OK | Issue clearly phases the work (verify gate → first-cut → full GeoCoder), names the blocking issue (#15), and scopes explicitly. No hidden side effects. | +| Enforcement over documentation | Watch | The sign-convention gate is a documentation/code-review step; there is no automated check to enforce it before the correction runs. A code comment and a test covering the sign (e.g., non-zero `beam_angle` beam should produce a corrected value ≠ raw) would strengthen this. | +| Capture decisions, not just implementations | Action needed | ADR-0007 (MBES Backscatter Store) is accepted and referenced in code/plans but has no document file in `cube_bathymetry/docs/decisions/`. When Phase B ends (this issue), the ADR-0007 transition from no-op to first-cut correction should be captured — either by writing the ADR-0007 document or adding an addendum noting the Phase B end date and approach. | +| A change includes its consequences | Action needed | Replacing the no-op with an actual Lambert correction changes settled intensity values. `test_node.cpp` tests that verify intensity output (asserting `corrected == raw`) will need updating to expect corrected values. Update tests in the same PR. | +| Only what's needed | OK | Issue explicitly limits scope to first-cut Lambert (no slope); the full GeoCoder correction is deferred. The change is a single-site replacement. | +| Improve incrementally | OK | Well-phased: Phase B no-op → first-cut Lambert (here) → full GeoCoder (#15). Small, reviewable. | +| Test what breaks | Action needed | (1) The Lambert correction needs at least one test with a non-nadir beam angle to confirm `corrected ≠ raw`. (2) Nadir-beam boundary case (`beam_angle ≈ 0`): correction should equal identity. (3) Confirm that `std::abs(beam_angle)` is used and a port beam and its symmetric starboard counterpart produce the same corrected value. | +| Workspace vs. project separation | OK | All changes in `cube_bathymetry` project repo. | + +### ADR Applicability + +| ADR | Triggered | Notes | +|---|---|---| +| ADR-0001 (workspace — adopt ADRs) | Yes | Implementing the deferred ADR-0007 D3 correction is a design decision transition; document it. | +| ADR-0008 (ROS 2 conventions) | No | No ROS 2 interface changes. | +| ADR-0013 (progress.md vocabulary) | Yes — this entry | Standard `## Issue Review` entry. | +| Project ADR-0007 (MBES Backscatter Store) | Yes — primary | D3 (incidence correction at node-output) is exactly what this issue implements. D2/D4 (per-beam combine into mean + estimate variance) are already in place from #54. | + +### Consequences + +- Settled backscatter values in `NodeRecord::intensity` will differ from the + current (uncorrected) values for any non-nadir beam — existing integration + tests that check intensity numerics will need updating. +- Both the live tile publisher (#78) and the offline store importer (#80) + automatically receive the corrected value via `extractNodeRecord()` — no + per-path duplication needed (the point of landing it here). +- The `TODO(#54-B / #15)` comment block at `node.cpp:298-322` should be + replaced or updated to reflect Phase B completion and note what the remaining + full-GeoCoder follow-up (#15) entails. +- If project ADR-0007 is written as part of this issue, the `docs/decisions/` + entry should be committed in the same PR. + +### Actions +- [ ] Document sign-convention cross-check (`rx_angles`: `+ starboard, - port` confirmed against `SonarDetections.msg` v2.1.0) as an inline comment at the correction site in `node.cpp`. +- [ ] Implement first-cut Lambert correction using `std::abs(sample.beam_angle)` (not the signed angle) to avoid port/starboard asymmetry. +- [ ] Update `test_node.cpp`: replace any intensity assertions that assume `corrected == raw` with expected Lambert-corrected values; add a test with a non-nadir beam angle confirming `corrected ≠ raw`. +- [ ] Add symmetric port/starboard test: equal `|beam_angle|` on each side should produce equal corrected intensity. +- [ ] Write or update project ADR-0007 document in `cube_bathymetry/docs/decisions/` to record the Phase B → first-cut transition (when it happens, what correction, what's still deferred). + +## Plan Authored +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Code Agent (Claude Sonnet) + +**Plan**: `cube_bathymetry/.agent/work-plans/issue-81/plan.md` at `94938a6` +**Branch**: feature/issue-81 at `94938a6` +**Phases**: single + +### Open questions +- [ ] Formula constant: confirm from Kongsberg EM Datagram Formats that `reflectivity_db` is raw backscatter without Lambert normalization in TVG, to avoid double-counting the cos θ correction. + +## Plan Review +**Status**: complete +**When**: 2026-06-28 10:50 +00:00 +**By**: Claude Code Agent (Claude Opus) + + +**Plan**: `.agent/work-plans/issue-81/plan.md` at `9efe563` +**PR**: PR-less (--issue mode) +**Verdict**: changes-requested + +### Findings +- [ ] (must-fix) Lambert constant unresolved + physically shaky justification: plan picks one-way `10·log10(cos θ)` arguing "transmit spreading is in TVG, only receive incidence is uncorrected" — but the Lambert `cos²θ` is a single seabed-scattering term (projected area × Lambertian emission), not a transmit/receive split, and TVG compensates range-dependent spreading/absorption, not angular incidence. Classic Lambert backscatter normalization is `20·log10(cos θ)` (=`10·log10(cos²θ)`). Resolve the load-bearing open question (`plan.md:149-153`) BEFORE implementing, not in parallel — `plan.md:31-47` +- [ ] (must-fix) Near-grazing guard is internally contradictory: derivation says "clamp θ to a maximum of MAX_INCIDENCE_RAD=80° before computing" (→ finite +correction at the cap) while the same bullet and the `LambertCorrectionNearGrazing` test say beams beyond max are "emitted uncorrected (identity)". Clamp-θ vs identity-beyond-max give different values and a discontinuity. Pick one, state it unambiguously, align the test — `plan.md:52-54`, `plan.md:89` +- [ ] (must-fix) Test-update list under-enumerates broken assertions: Step 3 names only `NodeRecordMeanAndEstimateVariance`, but two more non-nadir (0.1 rad) intensity assertions also break under the correction — `FirstBeamInitializationRecordsIntensity` (`test_node.cpp:382`, `EXPECT_FLOAT_EQ(record.intensity, -30.0f)`) and `NodeRecordSkipsNanIntensityBeam` (`test_node.cpp:420`). At 0.1 rad the shift is ≈0.022 dB, which fails both `FLOAT_EQ` and the `1e-4` `EXPECT_NEAR`. Enumerate all three. (Note: the variance assertion in `NodeRecordMeanAndEstimateVariance` still holds — all four beams share one angle, so the offset cancels; only the mean assertion changes.) — `plan.md:79-82` +- [ ] (suggestion) `test_store_import.cpp` Step 4 misses a stale comment: lines 64-67 explicitly assert "the surfaced value is uncorrected so the angle does not change it here" — that becomes false once the correction lands. Update the comment regardless of which fix is chosen. Prefer updating the assertion to the corrected value over zeroing `beam_angle` to 0.0f, so a non-nadir beam still exercises the offline-store path — `plan.md:91-97` +- [ ] (suggestion) The `grazing_angle → beam_angle` rename should also cover the header doc comments that mention the old name — `hypothesis.h:115-116` ("a NaN grazing_angle is retained") and the `BeamIntensitySample` reference at `hypothesis.h:166`; Step 1's file list cites only the field decl, recordBeam sig/impl, node.cpp:308, and tests — `plan.md:64-68` +- [ ] (suggestion) ADR-0007 transition-note filename `0007-mbes-backscatter-store-phase-b-transition.md` claims the `0007-` slot before the full ADR-0007 doc exists; confirm the planned follow-up's full doc won't collide (or intend it as a sibling addendum) — `plan.md:99-104` + +## Plan Review +**Status**: complete +**When**: 2026-06-28 11:20 +00:00 +**By**: Claude Code Agent (Claude Opus) + + +**Plan**: `.agent/work-plans/issue-81/plan.md` at `0387a24` +**PR**: PR-less (--issue mode) +**Verdict**: approve-with-suggestions + +Re-review after the plan was revised (`0387a24`) to address the prior +`changes-requested` review. All prior findings resolved, verified against live +source: ✓ formula → cos²θ / `−20·log10(cos θ)` (classic single-term Lambert seabed +law, replacing the unsound one-way ×10); ✓ near-grazing now a single unambiguous +rule (identity beyond 80°, no clamp/identity contradiction); ✓ all three broken +non-nadir assertions enumerated (`test_node.cpp:382`, the `:401` mean, +`:420`); ✓ `test_store_import` stale comment; ✓ rename covers `hypothesis.h:115-116` +and `:166`; ✓ addendum filename relabeled. Line numbers checked: no-op site +`node.cpp:315-318`, `grazing_angle` at `hypothesis.h:49`, `recordBeam` decl `:118` ++ impl `hypothesis.cpp:118/122/127`, both entry points present, ADR-0007 has no +file yet (only `0001-` exists). Formula has no objections. + +### Findings +- [ ] (suggestion) Step 3's "fix 3 broken non-nadir assertions" is now internally inconsistent with the default-off parameter design: under default `None`, `FirstBeamInitializationRecordsIntensity` (`test_node.cpp:382`), the `NodeRecordMeanAndEstimateVariance` mean (`:401`), and `NodeRecordSkipsNanIntensityBeam` (`:420`) do NOT break (`corrected == raw` when correction is off). They need no change; only the 6 new Lambert-ON tests assert corrected values. The "fix 3 broken" wording is carried over from the pre-parameter review — clarify that those three stay unchanged on the default path — `plan.md:112-121` +- [ ] (suggestion) Confirm the operator endorsed the **default-off** parameter design before implementing. It expands scope beyond the dispatched SCOPE BOUNDARY (which names no parameter) and changes #81's outcome: by default the no-op identity ships unchanged, so #78/#80 are corrected only when the flag is enabled. The plan attributes default-off to an operator decision (`plan.md:198-201`) but the dispatch's listed operator decisions cover only the gate-PASS and the ADR note. Design is defensible (sonar-agnostic; avoids double-counting a possibly pre-normalized reflectivity) — just verify the endorsement — `plan.md:24-29`, `plan.md:80-92` +- [ ] (suggestion, optional) Scope at upper bound (10 files). The `grazing_angle → beam_angle` rename is orthogonal to the correction and a clean split candidate if the PR grows large — not required — `plan.md:94-99` + +## Plan Authored +**Status**: complete +**When**: 2026-06-28 00:00 +00:00 +**By**: Claude Code Agent (Claude Sonnet) + +**Plan**: `.agent/work-plans/issue-81/plan.md` at `d1eacd3` +**Branch**: feature/issue-81 at `d1eacd3` +**Phases**: single + +### Open questions +- [ ] Script install method: `install(PROGRAMS ...)` in CMakeLists.txt vs `ament_python_install_package` for `derive_angular_response.py` — confirm house standard. +- [ ] Curve file format: CSV chosen (matches host-written seed) — confirm whether YAML is preferred for consistency with ROS parameter files. + +## Plan Review +**Status**: complete +**When**: 2026-06-28 12:23 +00:00 +**By**: Claude Code Agent (Claude Opus) + + +**Plan**: `.agent/work-plans/issue-81/plan.md` at `d1eacd3` +**PR**: PR-less (dispatched re-plan review) +**Verdict**: changes-requested + +Re-review of the new empirical-ARA plan (`d1eacd3`) that replaced the superseded +cos²θ plan. Verified against live source. The plan is thorough and faithful to the +operator-confirmed re-plan; all prior-review carry-overs are resolved (formula now +moot under ARA; default-off now operator-confirmed in dispatch; `|beam_angle|` use, +port/starboard symmetry test, and ADR addendum all present). One structural +file-targeting gap blocks implementation-readiness: the new correction parameters +have no plumbing path to the estimator. + +### Findings +- [ ] (must-fix) No API path from the app entry points to the new `Parameters` fields: `Parameters parameters_` is a **private** member of `GeoMapSheet` (`geo_map_sheet.h:102`), built internally from `(cell_size, iho_order)` (`geo_map_sheet.cpp:34`). Live node constructs the sheet with cell-size only (`cube_bathymetry_node.cpp:89`); offline tool with `(resolution, iho_order)` (`import_bag_main.cpp:411`). Step 2 ("load the file into `Parameters::angular_response_curve`") has nothing to write to. Add `geo_map_sheet.{h,cpp}` to Files-to-Change + Consequences with a ctor-arg-or-setter; decide whether the legacy Cartesian `MapSheet` path needs the same. Note: `test_node.cpp` calls `extractNodeRecord(params)` with a directly-built `Parameters` (`test_node.cpp:44,379`), so unit tests bypass this gap — it surfaces only as a no-effect correction in real wiring — `plan.md:60-71`, `plan.md:134-149` +- [ ] (suggestion) Stale comments beyond the listed ones: `import_bag_main.cpp:72-74` (`--bs-store` help, "UNCORRECTED intensity") and `test_store_import.cpp:271,303` ("uncorrected") should become "...by default" — plan cites only `test_store_import.cpp:64-67` — `plan.md:119-122` +- [ ] (suggestion) Rename should cover both `grazing_angle` sites in `node.cpp` — the prose at `:299` and `{raw_intensity, grazing_angle}` at `:308` (the only refs; params at `:71/:213` are already `beam_angle`) — `plan.md:40-47` +- [ ] (suggestion) `parameters.h` needs `` and `` added for the new `std::vector>` field (currently only ``,``,``) — `plan.md:49-58` + +### Next step +Lifecycle: **Plan Review** → **implement** → **review-code**. Verdict is +changes-requested: the implementer (or plan author) should add `geo_map_sheet.{h,cpp}` +plumbing to the plan and address the must-fix before/while implementing; the +suggestions are minor comment/include hygiene. + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-06-28 13:52 +00:00 +**By**: Claude Code Agent (Claude Opus) +**Verdict**: approved + +**Branch**: feature/issue-81 at `271e52f` +**Mode**: pre-push +**Depth**: Deep (reason: 1325 changed lines / 19 files / new ADR addendum) +**Must-fix**: 0 | **Suggestions**: 2 +**Round**: 1 | **Ship**: recommended — no must-fix; 2 minor suggestions (edge-clamp design + a misleading comment), all plan-review findings resolved + +### Findings +- [ ] (suggestion) `curveRelativeDb` clamp asymmetric: below-first-bin clamps to `curve.front().second` (assumes nadir bin), beyond-last-bin returns 0 (identity) creating a discontinuity at the outermost bin — clamp to `curve.back().second` or assert/doc the nadir-anchored curve — `cube_bathymetry/src/node.cpp:47` +- [ ] (suggestion) Loader comment overstates the `a_used`/`d_used` guard: `std::stof` parses a numeric prefix, so `"1deg"` is accepted as `1.0`, not rejected — comment-accuracy nit — `cube_bathymetry/src/angular_response_curve.cpp:93` + +### Notes +- Base scope: local `gitcloud/jazzy` mirror is stale (predates merged #80 / PR #82 at `7467e1b`); review scoped to the #81-only diff `7467e1b...HEAD` (19 files, +1325 −39). The bundled #80 portion was already reviewed/integrated. +- Deep review: 2 fresh-context Claude Adversarial passes (Lens A logic + Lens B systemic) — both clean, no must-fix. Cross-pass confirmation on the curve-edge clamp (suggestion 1). Copilot off (default). +- Static analysis clean: ament_cpplint, ament_uncrustify, ament_cppcheck (slow-version override), ament_flake8 (Python tool) all "No problems found". +- Tests **pass** against built binaries: `NodeTest.ARA*` (6/6) and `AngularResponseCurve.*` (4/4). +- Verified correct: rad→deg units + sign convention (`std::abs(beam_angle)`); interpolation incl. duplicate-angle (`span<=0`) guard; mean + variance-of-mean math (clamped ≥0); Python writer columns (0=center,3=db_rel) match C++ loader reads; correction applied once at `extractNodeRecord` (warm-start/eviction reload reseeds depth only — no double-correction); SingleThreadedExecutor → no param-mutation race. +- Plan adherence: full. Plan-review must-fix (geo_map_sheet plumbing) resolved; setter ordering verified before any grid processes soundings on both live + offline paths. ``/`` includes present. +- Pre-existing (not a #81 finding): configure→cleanup→configure would throw `ParameterAlreadyDeclaredException` — node-wide pattern, not introduced here. + +### Next step +Lifecycle: **Local Review** → push / open PR → **triage-reviews**. Verdict is +**approved** with no must-fix — the diff is shippable; the 2 suggestions can be +applied or tracked. Next phase is dispatched by the host (`/run-issue`). diff --git a/cube_bathymetry/CMakeLists.txt b/cube_bathymetry/CMakeLists.txt index 4c4c41d..2dc4667 100644 --- a/cube_bathymetry/CMakeLists.txt +++ b/cube_bathymetry/CMakeLists.txt @@ -43,6 +43,7 @@ find_package(tf2_sensor_msgs REQUIRED) find_package(GDAL CONFIG REQUIRED) add_library(cube_bathymetry + src/angular_response_curve.cpp src/detections_projector.cpp src/error_model.cpp src/geo_grid.cpp @@ -142,6 +143,14 @@ install(TARGETS cube_bathymetry_node install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# Offline empirical angular-response curve derivation tool (cube#81). Reads a +# SonarDetections bag, bins intensities by |rx_angles|, writes the curve CSV the +# Empirical backscatter correction consumes. +install(PROGRAMS + scripts/derive_angular_response.py + DESTINATION lib/${PROJECT_NAME} +) + add_executable(detections_to_pointcloud src/detections_to_pointcloud.cpp) target_link_libraries(detections_to_pointcloud @@ -212,6 +221,9 @@ if(BUILD_TESTING) ament_add_gtest(test_node test/test_node.cpp) target_link_libraries(test_node cube_bathymetry) + ament_add_gtest(test_angular_response_curve test/test_angular_response_curve.cpp) + target_link_libraries(test_angular_response_curve cube_bathymetry) + ament_add_gtest(test_parameters test/test_parameters.cpp) target_link_libraries(test_parameters cube_bathymetry) 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 new file mode 100644 index 0000000..95b66f8 --- /dev/null +++ b/cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md @@ -0,0 +1,103 @@ +# ADR-0007 (addendum): MBES backscatter — Phase B transition to empirical angular-response correction + +## Status + +Accepted (addendum). + +This is an **addendum**, not the canonical ADR-0007 document. The full +`0007-mbes-backscatter-store.md` (the per-beam co-estimation design and the +"D1–D5" decision points referenced throughout the `cube_bathymetry` sources) has +not yet been authored as a committed file in `docs/decisions/`; the design lives +in the source comments and the earlier `unh_marine_autonomy` ADR set. This file +records one concrete transition — the end of "Phase B" (the identity no-op) and +the introduction of an **empirical angular-response (ARA) correction** — so the +decision is captured now rather than lost. A follow-up will author the full +ADR-0007 document and renumber/fold this addendum into it. It deliberately does +**not** claim the canonical `0007` slot's content. + +## Context + +`Node::extractNodeRecord()` retains, per winning depth hypothesis, the per-beam +`{raw_intensity, beam_angle}` sufficient statistics (ADR-0007 D2/D3/D4). Until +now the node-output correction was a **Phase B no-op**: `corrected == raw` for +every beam, with the per-beam pairs kept so the surfaced backscatter stays +re-derivable once a real correction lands. + +An empirical study of the `m3_dryrun` Portsmouth Harbor survey (`/detections`, +2025 pings, ~445k beams) showed the earlier candidate model — a cos²θ Lambert +correction — is wrong for this data: it explains only ~¼ of the observed +nadir-to-edge falloff (~24 dB). The remainder is dominated by sonar-equation +terms cos²θ ignores (insonified area, beam-pattern, TVG residual). A +closed-form per-beam model is therefore not justified by the data on hand. + +The angular dependence *is*, however, well captured empirically: binning real +beams by `|rx_angle|` produces a smooth, monotonic angular-response curve per +sonar. The `rx_angles` sign/zero convention was verified against +`kongsberg_em_bridge`: the producer negates the Kongsberg pointing angle +(+PORT → +STARBOARD), nadir = 0, radians, intensity in dB. + +## Decision + +1. **Replace the Phase B no-op with an empirical-ARA correction** at + `Node::extractNodeRecord()`: + `corrected_dB = raw_dB − curveRel(|beam_angle|)`, where `curveRel` is a + per-sonar angular-response curve's `db_relative_to_nadir` column, **linearly + interpolated between bin centres** by `|beam_angle|` in **degrees**. Nadir → + `curveRel ≈ 0` → identity. Beyond the curve's max angle, a NaN `beam_angle`, + mode `None`, or an empty curve all fall back to identity (raw). The curve is + keyed on `|angle|`, so port/starboard beams of equal magnitude get the same + correction. + +2. **Configured curve, default OFF, sonar-agnostic.** No correction model is + baked into the estimator (which serves M3, Norbit, R2Sonic, sim, …). The mode + (`none` | `empirical`) and the curve file are operator-supplied at startup; + the default `none` preserves the prior identity behavior on every existing + deployment. An `empirical` mode with an empty/missing/unparseable curve is a + **loud no-op** — a WARNING is logged (node `RCLCPP_WARN`, offline `std::cerr`) + so the misconfiguration is visible rather than silent. + +3. **Plumbed through both estimator entry points** — the live node + (`backscatter_angle_correction` / `backscatter_curve_file` ROS params) and the + offline importer (`--backscatter-correction` / `--backscatter-curve`). Both + run the same estimator, so one mechanism corrects both the live and offline + paths. The mode + curve are written into the sheet's private `Parameters` via + `GeoMapSheet::setBackscatterCorrection()`; grids hold a `const Parameters&`, + so the setting reaches every grid (call after construction, before/at + processing). + +4. **Offline derivation tool.** `scripts/derive_angular_response.py` reads a + `SonarDetections` bag, bins `intensities` by `|rx_angles|` (2° bins by + default), and writes the curve CSV + (`abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir`). This is how an + operator produces a curve for their sonar. + +5. **Rename** `BeamIntensitySample::grazing_angle` → `beam_angle`. The field + stores the receive/steering (beam/incidence) angle from nadir, not a true + grazing angle (90° − θ). + +## Consequences + +- The surfaced backscatter (live `grid`/`~/tiles` and the offline `--bs-store` + layer) is **uncorrected by default**; it becomes angle-corrected only when an + operator opts in and supplies a curve. Documentation and the importer `usage()` + reflect "uncorrected by default". +- The per-beam `{raw_intensity, beam_angle}` retention is unchanged, so the node + value stays fully re-derivable when a richer correction lands. +- Tests: 6 ARA cases at `extractNodeRecord()` (interpolation, nadir identity, + beyond-max identity, NaN-angle identity, mode-None identity, port/starboard + symmetry) plus curve-loader/mode-parse tests; the 3 pre-existing default-path + intensity assertions are unchanged (default `None` ⇒ `corrected == raw`). + +## 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 + 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). +- **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 + the intensity domain as a sonar property rather than assuming dB. diff --git a/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h b/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h new file mode 100644 index 0000000..c8e9cb3 --- /dev/null +++ b/cube_bathymetry/include/cube_bathymetry/angular_response_curve.h @@ -0,0 +1,53 @@ +// Copyright 2025 Center for Coastal and Ocean Mapping & NOAA-UNH Joint +// Hydrographic Center, University of New Hampshire +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + + +#ifndef CUBE_BATHYMETRY__ANGULAR_RESPONSE_CURVE_H_ +#define CUBE_BATHYMETRY__ANGULAR_RESPONSE_CURVE_H_ + +#include +#include +#include +#include "cube_bathymetry/parameters.h" + +namespace cube +{ + +/// Parse a backscatter angular-response correction mode string ("none" or +/// "empirical", case-insensitive). Returns false on an unrecognized value +/// (out is left unchanged); the caller decides how to report the error. + bool parseBackscatterAngleCorrection( + const std::string & text, BackscatterAngleCorrection & out); + +/// 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). +/// +/// Comment lines (`#`...), blank lines, the header row, and rows that fail to +/// parse are skipped. A missing/unreadable file yields an empty vector (the +/// Empirical correction then degrades to a no-op -- callers should warn). + std::vector < std::pair < float, float >> loadAngularResponseCurve(const std::string & path); + +} // namespace cube + +#endif // CUBE_BATHYMETRY__ANGULAR_RESPONSE_CURVE_H_ diff --git a/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h b/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h index 34668ae..7593df5 100644 --- a/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h +++ b/cube_bathymetry/include/cube_bathymetry/geo_map_sheet.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include "cube_bathymetry/geo_grid.h" @@ -49,6 +50,17 @@ namespace cube const std::vector < GeoSounding > &soundings, std::chrono::steady_clock::time_point time = std::chrono::steady_clock::now()); + /// @brief Configure the per-beam backscatter angular-response correction + /// applied at node-output (ADR-0007 D3, cube_bathymetry#81). + /// + /// Writes @p mode and @p curve into the sheet's private @c parameters_, which + /// 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). + void setBackscatterCorrection( + BackscatterAngleCorrection mode, + std::vector < std::pair < float, float >> curve); + /// Return the grids within the bounds, creating new ones if necessary std::vector < std::shared_ptr < GeoGrid >> getOrCreateGridsIn(const gz4d::BoundsDegrees & bounds); diff --git a/cube_bathymetry/include/cube_bathymetry/hypothesis.h b/cube_bathymetry/include/cube_bathymetry/hypothesis.h index ada305a..8b6e0e4 100644 --- a/cube_bathymetry/include/cube_bathymetry/hypothesis.h +++ b/cube_bathymetry/include/cube_bathymetry/hypothesis.h @@ -33,8 +33,8 @@ namespace cube /// Per-beam backscatter sufficient statistics retained on a hypothesis /// (ADR-0007 D3). Each contributing beam contributes its RAW (uncorrected) -/// intensity and the per-beam grazing/beam angle, so the radiometric -/// (GeoCoder incidence/Lambert) correction can be applied later -- at +/// intensity and the per-beam receive/steering (beam/incidence) angle, so the +/// radiometric (empirical-ARA / GeoCoder) correction can be applied later -- at /// node-output, once depth and local slope have settled -- rather than baking /// an angle-corrupted, non-re-correctable average into the hypothesis. struct BeamIntensitySample @@ -42,11 +42,12 @@ namespace cube /// Raw, uncorrected per-beam intensity (e.g. M3 reflectivity in dB). float raw_intensity; - /// Per-beam receive/steering (beam/incidence) angle in radians. May be NaN - /// when the source did not report an angle for the beam; the deferred output - /// correction treats a NaN angle as "no angle correction available" and emits - /// that beam uncorrected. - float grazing_angle; + /// Per-beam receive/steering (beam/incidence) angle from nadir, in radians + /// (NOT a true grazing angle of 90 deg - theta; nadir = 0). May be NaN when + /// the source did not report an angle for the beam; the output correction + /// treats a NaN angle as "no angle correction available" and emits that beam + /// uncorrected. + float beam_angle; }; /// Depth hypothesis structure used to maintain a current track on the depth @@ -113,9 +114,9 @@ namespace cube /// outliers the W&H monitor rejects never enter this set. /// /// A NaN raw_intensity is skipped (a source that omits intensities must never - /// inject a phantom sample); a NaN grazing_angle is retained (the beam is + /// 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 grazing_angle); + void recordBeam(float raw_intensity, float beam_angle); /// Current depth mean estimate double current_estimate; diff --git a/cube_bathymetry/include/cube_bathymetry/parameters.h b/cube_bathymetry/include/cube_bathymetry/parameters.h index 04c3472..cbb5aa7 100644 --- a/cube_bathymetry/include/cube_bathymetry/parameters.h +++ b/cube_bathymetry/include/cube_bathymetry/parameters.h @@ -26,11 +26,27 @@ #include #include #include +#include +#include #include "cube_bathymetry/common.h" namespace cube { +/// Per-beam backscatter angular-response correction applied at node-output +/// (Node::extractNodeRecord, ADR-0007 D3). Default None preserves the Phase B +/// identity behavior (corrected == raw) on every existing deployment. + enum class BackscatterAngleCorrection + { + /// No correction: the surfaced intensity is the raw per-beam mean. + None, + + /// Empirical angular-response (ARA): subtract a per-sonar curve's + /// db_relative_to_nadir, linearly interpolated by |beam_angle| in degrees. + /// Requires a non-empty angular_response_curve; empty curve -> no-op. + Empirical + }; + /// Extraction method for depth and uncertainty surfaces. This is used only in /// the multiple hypothesis case as a way of choosing which hypothesis to /// extract and report as the `current best guess'. @@ -162,6 +178,18 @@ namespace cube /// hydrography but can be greater for geological mapping /// in flat areas with sparse data) float capture_distance_scale = 0.05; + + /// Per-beam backscatter angular-response correction mode (ADR-0007 D3). + /// Default None = identity (corrected == raw). Set via + /// GeoMapSheet::setBackscatterCorrection(). + BackscatterAngleCorrection backscatter_angle_correction = + BackscatterAngleCorrection::None; + + /// Empirical angular-response curve: ascending {abs_angle_deg, db_relative_to_nadir} + /// pairs (nadir bin ~0, off-nadir bins <= 0), loaded at startup from a per-sonar + /// 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; }; } // namespace cube diff --git a/cube_bathymetry/scripts/derive_angular_response.py b/cube_bathymetry/scripts/derive_angular_response.py new file mode 100755 index 0000000..b86cdca --- /dev/null +++ b/cube_bathymetry/scripts/derive_angular_response.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +# Copyright 2025 Center for Coastal and Ocean Mapping & NOAA-UNH Joint +# Hydrographic Center, University of New Hampshire +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +""" +Derive an empirical backscatter angular-response curve from a bag. + +Reads a marine_acoustic_msgs/SonarDetections topic, bins each beam's +``intensities`` (dB) by ``|rx_angles|`` (converted to degrees) into fixed-width +bins, and writes a CSV with the columns consumed by the CUBE estimator's +empirical ARA correction (cube_bathymetry#81): + + abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir + +``db_relative_to_nadir`` is ``mean_bs_db - nadir_bin_mean_bs_db`` so the nadir +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. + +Usage: + derive_angular_response.py -t -o +""" + +import argparse +import math +import sys + + +def parse_args(argv): + p = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument('bag', help='rosbag2 directory (or .mcap/.db3 file)') + 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) + + +def open_reader(bag_path): + """Open a rosbag2 SequentialReader, auto-detecting the storage backend.""" + import rosbag2_py + + storage_id = '' + if bag_path.endswith('.mcap'): + storage_id = 'mcap' + elif bag_path.endswith('.db3'): + storage_id = 'sqlite3' + storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id=storage_id) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format='cdr', output_serialization_format='cdr') + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + return reader + + +def accumulate(reader, topic, bin_width_deg): + """Return {bin_index: [sum_db, n]} for finite intensities on `topic`.""" + from rclpy.serialization import deserialize_message + from rosidl_runtime_py.utilities import get_message + + type_map = {t.name: t.type for t in reader.get_all_topics_and_types()} + if topic not in type_map: + raise SystemExit( + "error: topic '%s' not in bag (have: %s)" + % (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() + if name != topic: + continue + msg = deserialize_message(data, msg_type) + n_msgs += 1 + intensities = msg.intensities + rx_angles = msg.rx_angles + flags = getattr(msg, 'flags', None) + count = min(len(intensities), len(rx_angles)) + for i in range(count): + db = intensities[i] + if not math.isfinite(db): + continue + # Skip beams flagged bad (flag != 0) when flags are present and aligned. + if flags is not None and i < len(flags): + flag_val = getattr(flags[i], 'flag', 0) + if flag_val != 0: + continue + ang = rx_angles[i] + if not math.isfinite(ang): + continue + 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 + + +def write_csv(bins, bin_width_deg, out_path): + """Write the binned means and db_relative_to_nadir to a curve CSV.""" + if not bins: + raise SystemExit('error: no finite intensity/angle samples found') + + # Per-bin mean dB at the bin centre. + rows = [] + for idx in sorted(bins): + s, n = bins[idx] + center = (idx + 0.5) * bin_width_deg + rows.append([center, s / n, 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') + 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 + f.write('%.1f,%.3f,%d,%+.3f\n' % (center, mean_db, n, db_rel)) + return len(rows) + + +def main(argv=None): + """Read the bag, bin by |rx_angles|, and write the curve CSV.""" + 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)) + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/cube_bathymetry/src/angular_response_curve.cpp b/cube_bathymetry/src/angular_response_curve.cpp new file mode 100644 index 0000000..0a12f5e --- /dev/null +++ b/cube_bathymetry/src/angular_response_curve.cpp @@ -0,0 +1,119 @@ +// Copyright 2025 Center for Coastal and Ocean Mapping & NOAA-UNH Joint +// Hydrographic Center, University of New Hampshire +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + + +#include "cube_bathymetry/angular_response_curve.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace cube +{ + +bool parseBackscatterAngleCorrection( + const std::string & text, BackscatterAngleCorrection & out) +{ + std::string lower; + lower.reserve(text.size()); + for (char c : text) { + lower.push_back(static_cast(std::tolower(static_cast(c)))); + } + if (lower == "none" || lower.empty()) { + out = BackscatterAngleCorrection::None; + return true; + } + if (lower == "empirical") { + out = BackscatterAngleCorrection::Empirical; + return true; + } + return false; +} + +std::vector> loadAngularResponseCurve(const std::string & path) +{ + std::vector> curve; + if (path.empty()) { + return curve; + } + + std::ifstream in(path); + if (!in) { + return curve; + } + + std::string line; + while (std::getline(in, line)) { + // Skip blank lines and comments. + const auto first = line.find_first_not_of(" \t\r\n"); + if (first == std::string::npos || line[first] == '#') { + continue; + } + + // Split on commas. We need column 0 (abs_angle_deg_center) and column 3 + // (db_relative_to_nadir). A header row or any malformed row fails the float + // parse and is skipped. + std::vector fields; + std::stringstream ss(line); + std::string field; + while (std::getline(ss, field, ',')) { + fields.push_back(field); + } + if (fields.size() < 4) { + continue; + } + + try { + std::size_t a_used = 0; + std::size_t d_used = 0; + const float angle = std::stof(fields[0], &a_used); + const float db_rel = std::stof(fields[3], &d_used); + // std::stof throws on a field with no leading number (e.g. the header's + // "abs_angle_deg_center"), handled by the catch below. The used==0 check is + // a belt-and-suspenders guard for an empty parse. Note a value like "1deg" + // parses its numeric prefix (1.0) and IS accepted -- the derive tool writes + // clean numeric columns, so trailing garbage is tolerated, not rejected. + if (a_used == 0 || d_used == 0) { + continue; + } + curve.emplace_back(angle, db_rel); + } catch (const std::exception &) { + // Header row or malformed numeric -- skip. + continue; + } + } + + // Ensure ascending order by angle so the interpolation can assume monotonic + // bin centres regardless of file ordering. + std::sort( + curve.begin(), curve.end(), + [](const std::pair & a, const std::pair & b) { + return a.first < b.first; + }); + + return curve; +} + +} // namespace cube diff --git a/cube_bathymetry/src/cube_bathymetry_node.cpp b/cube_bathymetry/src/cube_bathymetry_node.cpp index 3faac65..f0950ab 100644 --- a/cube_bathymetry/src/cube_bathymetry_node.cpp +++ b/cube_bathymetry/src/cube_bathymetry_node.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include "tf2_ros/transform_listener.h" @@ -49,6 +50,7 @@ #include "grid_map_msgs/msg/grid_map.hpp" #include "marine_autonomy/gz4d_geo.h" +#include "cube_bathymetry/angular_response_curve.h" #include "cube_bathymetry/store_import.h" #include "marine_bathymetry_store/bathymetry_store.hpp" #include "marine_bathymetry_store/bathymetry_tile.hpp" @@ -91,6 +93,44 @@ class CubeBathymetry : public rclcpp_lifecycle::LifecycleNode // configure cycle so a stale index can't trigger a spurious reload (#70 r2). evicted_indices_.clear(); + // Backscatter angular-response correction (cube_bathymetry#81). Default + // "none" preserves the Phase B identity behavior; "empirical" subtracts a + // per-sonar angular-response curve loaded from backscatter_curve_file. The + // setter must run AFTER the sheet is constructed (grids hold a const ref to + // the sheet's Parameters). + const std::string bs_correction_str = + declare_parameter("backscatter_angle_correction", std::string("none")); + const std::string bs_curve_file = + declare_parameter("backscatter_curve_file", std::string("")); + cube::BackscatterAngleCorrection bs_mode = + cube::BackscatterAngleCorrection::None; + if (!cube::parseBackscatterAngleCorrection(bs_correction_str, bs_mode)) { + RCLCPP_WARN(get_logger(), + "backscatter_angle_correction='%s' is not 'none' or 'empirical'; " + "defaulting to none (no correction).", bs_correction_str.c_str()); + bs_mode = cube::BackscatterAngleCorrection::None; + } + std::vector> bs_curve; + if (bs_mode == cube::BackscatterAngleCorrection::Empirical && + !bs_curve_file.empty()) + { + bs_curve = cube::loadAngularResponseCurve(bs_curve_file); + } + if (bs_mode == cube::BackscatterAngleCorrection::Empirical && bs_curve.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(), + "backscatter_angle_correction=empirical but no curve was loaded from " + "backscatter_curve_file='%s' -- the correction is ENABLED but a NO-OP " + "(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()); + } + geo_map_sheet_->setBackscatterCorrection(bs_mode, std::move(bs_curve)); + // Long-duration bounding (#70, ADR-0001). Declared BEFORE the draft prime so // the prime can be trimmed to the same budget -- otherwise loadIntoSheet // (whole store) re-creates the unbounded-RAM condition #70 exists to prevent. diff --git a/cube_bathymetry/src/geo_map_sheet.cpp b/cube_bathymetry/src/geo_map_sheet.cpp index 925484e..b3f8143 100644 --- a/cube_bathymetry/src/geo_map_sheet.cpp +++ b/cube_bathymetry/src/geo_map_sheet.cpp @@ -35,6 +35,15 @@ GeoMapSheet::GeoMapSheet(float cell_size, std::string iho_order) { } +void GeoMapSheet::setBackscatterCorrection( + BackscatterAngleCorrection mode, + std::vector> curve) +{ + // 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); +} + void GeoMapSheet::addSoundings( const std::vector & soundings, std::chrono::steady_clock::time_point time) diff --git a/cube_bathymetry/src/hypothesis.cpp b/cube_bathymetry/src/hypothesis.cpp index 49bf04b..25a99e3 100644 --- a/cube_bathymetry/src/hypothesis.cpp +++ b/cube_bathymetry/src/hypothesis.cpp @@ -115,16 +115,16 @@ bool Hypothesis::update(float depth, float variance, const Parameters & paramete return true; } -void Hypothesis::recordBeam(float raw_intensity, float grazing_angle) +void Hypothesis::recordBeam(float raw_intensity, float beam_angle) { // 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 grazing_angle is allowed through: the beam is still a valid intensity + // A NaN beam_angle is allowed through: the beam is still a valid intensity // measurement, it merely cannot be angle-corrected at output. if (std::isnan(raw_intensity)) { return; } - intensity_samples.push_back(BeamIntensitySample{raw_intensity, grazing_angle}); + intensity_samples.push_back(BeamIntensitySample{raw_intensity, beam_angle}); } diff --git a/cube_bathymetry/src/import_bag_main.cpp b/cube_bathymetry/src/import_bag_main.cpp index 2b672ec..1fd959a 100644 --- a/cube_bathymetry/src/import_bag_main.cpp +++ b/cube_bathymetry/src/import_bag_main.cpp @@ -43,6 +43,7 @@ #include #include +#include "cube_bathymetry/angular_response_curve.h" #include "cube_bathymetry/detections_projector.h" #include "cube_bathymetry/geo_map_sheet.h" #include "cube_bathymetry/geo_sounding.h" @@ -70,8 +71,9 @@ std::cout << " -o : Output bathymetry-store directory (created if " "needed)\n"; std::cout << " --bs-store : Also write an MBES backscatter store layer " - "(Processed) from the same CUBE pass (optional; surfaces the co-estimated, " - "UNCORRECTED intensity -- the angle correction is cube#81)\n"; + "(Processed) from the same CUBE pass (optional; surfaces the co-estimated " + "intensity -- uncorrected by default, angle-corrected when " + "--backscatter-correction empirical is set, cube#81)\n"; std::cout << " -d : marine_acoustic_msgs/SonarDetections " "topic to replay through CUBE (required)\n"; std::cout << " --odom-topic : nav_msgs/Odometry topic for per-ping " @@ -80,6 +82,12 @@ std::cout << " -r : Grid resolution (nominal; snapped to GGGS). " "Default 1.0\n"; std::cout << " --iho-order : CUBE IHO order (default order1a)\n"; + std::cout << " --backscatter-correction none|empirical: per-beam angular-response " + "correction at node-output (default none = identity). 'empirical' subtracts the " + "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"; 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"; @@ -271,6 +279,9 @@ int main(int argc, char * argv[]) double resolution = 1.0; std::string iho_order = "order1a"; int ping_count_limit = 0; + // Backscatter angular-response correction (cube#81). Default none = identity. + std::string backscatter_correction_str = "none"; + std::string backscatter_curve_file; // Registry provenance fields for the imported cells. marine_bathymetry_store::SourceRecord source_record; @@ -311,6 +322,10 @@ int main(int argc, char * argv[]) resolution = std::stod(next_value("-r")); } else if (*arg == "--iho-order") { iho_order = next_value("--iho-order"); + } else if (*arg == "--backscatter-correction") { + backscatter_correction_str = next_value("--backscatter-correction"); + } else if (*arg == "--backscatter-curve") { + backscatter_curve_file = next_value("--backscatter-curve"); } else if (*arg == "-l") { ping_count_limit = std::stoi(next_value("-l")); } else if (*arg == "--source-id") { @@ -412,6 +427,39 @@ int main(int argc, char * argv[]) std::cout << "requested resolution: " << resolution << " nominal used: " << geo_map_sheet.nominalCellSizeMeters() << std::endl; + // Backscatter angular-response correction (cube#81). The setter must run AFTER + // the sheet is constructed (its grids hold a const ref to the sheet Parameters). + cube::BackscatterAngleCorrection backscatter_mode = + cube::BackscatterAngleCorrection::None; + if (!cube::parseBackscatterAngleCorrection( + backscatter_correction_str, backscatter_mode)) + { + std::cerr << "error: --backscatter-correction must be 'none' or 'empirical' " + << "(got '" << backscatter_correction_str << "')\n"; + usage(); + } + std::vector> backscatter_curve; + if (backscatter_mode == cube::BackscatterAngleCorrection::Empirical && + !backscatter_curve_file.empty()) + { + backscatter_curve = cube::loadAngularResponseCurve(backscatter_curve_file); + } + if (backscatter_mode == cube::BackscatterAngleCorrection::Empirical && + backscatter_curve.empty()) + { + // Loud, not silent: enabled but no curve loaded -> correction is a no-op. + std::cerr << "warning: --backscatter-correction empirical but no curve was " + "loaded from --backscatter-curve '" << backscatter_curve_file + << "' -- the correction is ENABLED but a NO-OP (intensity emitted " + "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; + } + geo_map_sheet.setBackscatterCorrection( + backscatter_mode, std::move(backscatter_curve)); + std::cout << "reading messages..." << std::endl; int ping_count = 0; diff --git a/cube_bathymetry/src/node.cpp b/cube_bathymetry/src/node.cpp index 7db12a1..420bd71 100644 --- a/cube_bathymetry/src/node.cpp +++ b/cube_bathymetry/src/node.cpp @@ -23,10 +23,58 @@ #include "cube_bathymetry/node.h" #include #include +#include +#include namespace cube { +namespace +{ + +/// Empirical angular-response lookup: the curve's db_relative_to_nadir at +/// @p abs_angle_deg, linearly interpolated between adjacent bin centres. The +/// curve is ascending {abs_angle_deg, db_relative_to_nadir} pairs (loaded by +/// loadAngularResponseCurve). Returns 0 (identity) when the curve is empty or +/// @p abs_angle_deg lies beyond the curve's max angle; clamps to the first bin +/// below the curve's min angle (nadir bin ~0 -> ~identity near nadir). +double curveRelativeDb( + const std::vector> & curve, double abs_angle_deg) +{ + if (curve.empty()) { + return 0.0; + } + // Below the first bin centre: clamp to it (the nadir bin's value, ~0). + if (abs_angle_deg <= curve.front().first) { + return curve.front().second; + } + // Beyond the last bin centre: clamp to the outermost bin's correction rather + // than jump to identity. The empirical curve is bounded (unlike a cos/log model + // that diverges near grazing), so continuing the edge value keeps the correction + // continuous and avoids a swath-edge discontinuity / bright ring (#81 review). + if (abs_angle_deg > curve.back().first) { + return curve.back().second; + } + // Find the bracketing pair [lo, hi] and linearly interpolate. + for (std::size_t i = 1; i < curve.size(); ++i) { + if (abs_angle_deg <= curve[i].first) { + const double a0 = curve[i - 1].first; + const double d0 = curve[i - 1].second; + const double a1 = curve[i].first; + const double d1 = curve[i].second; + const double span = a1 - a0; + if (span <= 0.0) { + return d1; // duplicate angle: take the upper bin's value + } + const double t = (abs_angle_deg - a0) / span; + return d0 + t * (d1 - d0); + } + } + return 0.0; // unreachable (guarded by the back() check above) +} + +} // namespace + bool Node::addHypothesis(float depth, float variance) { auto new_hypothesis = std::make_shared(depth, variance); @@ -295,27 +343,41 @@ NodeRecord Node::extractNodeRecord(const Parameters & parameters) return record; } - // Backscatter half (ADR-0007 D2/D3/D4). Apply the per-beam radiometric - // correction to each retained {raw intensity, grazing angle} sample, THEN - // combine the corrected values into a mean and an ESTIMATE variance. + // Backscatter half (ADR-0007 D2/D3/D4). Apply the per-beam angular-response + // correction to each retained {raw_intensity, beam_angle} sample, THEN combine + // the corrected values into a mean and an ESTIMATE variance. // - // TODO(#54-B / cube_bathymetry#15): apply the GeoCoder incidence/Lambert - // correction per beam HERE, using the winning hypothesis's settled depth and - // the local seabed slope (ADR-0007 D3). The slope correction (cube_bathymetry#15) - // has landed in Node::insert but is inert (offset 0) until its predicted-surface - // producer (cube_bathymetry#59) is wired. Until that producer lands this is the - // identity (flat-geometry) correction: the corrected value equals the raw value and - // intensity is emitted UNCORRECTED. The per-beam {raw_intensity, grazing_angle} - // set is retained on the hypothesis (Hypothesis::intensity_samples) so the - // node value is fully re-derivable when #15 provides slope -- no information - // is lost by deferring. + // Empirical ARA (cube_bathymetry#81): corrected_dB = raw_dB - curveRel(|beam_angle|), + // where curveRel is the per-sonar angular-response curve's db_relative_to_nadir + // column, linearly interpolated between bin centres by |beam_angle| in DEGREES. + // Nadir -> curveRel ~0 -> identity. Beyond the curve's max angle, a NaN + // beam_angle, mode None, or an empty curve all fall back to identity (raw). + // + // rx_angles sign/zero gate (verified against kongsberg_em_bridge): the producer + // negates the Kongsberg pointing angle (+PORT -> +STARBOARD; nadir = 0; radians; + // intensity in dB). The curve is keyed on |beam_angle|, so port/starboard beams + // of equal magnitude get the same correction. + // + // 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. + const bool apply_ara = + parameters.backscatter_angle_correction == + BackscatterAngleCorrection::Empirical && + !parameters.angular_response_curve.empty(); double sum = 0.0; double sum_sq = 0.0; uint32_t n = 0; for(const auto & sample : chosen->intensity_samples) { - // Phase B no-op correction: corrected == raw for now. recordBeam() already - // excluded NaN-intensity beams, so every retained sample is a real value. - const double corrected = sample.raw_intensity; + // recordBeam() already excluded NaN-intensity beams, so raw_intensity is real. + double corrected = sample.raw_intensity; + if(apply_ara && !std::isnan(sample.beam_angle)) { + const double abs_angle_deg = + std::abs(static_cast(sample.beam_angle)) * 180.0 / M_PI; + corrected -= curveRelativeDb(parameters.angular_response_curve, abs_angle_deg); + } sum += corrected; sum_sq += corrected * corrected; ++n; diff --git a/cube_bathymetry/test/test_angular_response_curve.cpp b/cube_bathymetry/test/test_angular_response_curve.cpp new file mode 100644 index 0000000..290fc17 --- /dev/null +++ b/cube_bathymetry/test/test_angular_response_curve.cpp @@ -0,0 +1,114 @@ +// Copyright 2025 Center for Coastal and Ocean Mapping and NOAA-UNH Joint +// Hydrographic Center, University of New Hampshire +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include +#include +#include "cube_bathymetry/angular_response_curve.h" + +namespace cube +{ + +namespace +{ +// Write `content` to a unique temp file; the path is returned for loading and +// removed by the caller. +std::string writeTemp(const std::string & content) +{ + std::string path = std::string(std::tmpnam(nullptr)) + ".csv"; + std::ofstream out(path); + out << content; + out.close(); + return path; +} +} // namespace + +// A well-formed curve CSV (header + comment + data rows, like the M3 seed) parses +// into ascending {abs_angle_deg, db_relative_to_nadir} pairs, reading columns 0 +// and 3 and skipping the header and comment lines. +TEST(AngularResponseCurve, ParsesSeedFormat) +{ + 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.155,17984,+0.000\n" + "3.0,-40.328,19986,-0.173\n" + "5.0,-40.971,15999,-0.816\n"; + const std::string path = writeTemp(csv); + + auto curve = loadAngularResponseCurve(path); + std::remove(path.c_str()); + + ASSERT_EQ(curve.size(), 3u); + EXPECT_FLOAT_EQ(curve[0].first, 1.0f); + EXPECT_FLOAT_EQ(curve[0].second, 0.0f); + EXPECT_FLOAT_EQ(curve[1].first, 3.0f); + EXPECT_FLOAT_EQ(curve[1].second, -0.173f); + EXPECT_FLOAT_EQ(curve[2].first, 5.0f); + EXPECT_FLOAT_EQ(curve[2].second, -0.816f); +} + +// Out-of-order rows are sorted ascending by angle; malformed rows are skipped. +TEST(AngularResponseCurve, SortsAndSkipsMalformed) +{ + const std::string csv = + "abs_angle_deg_center,mean_bs_db,n,db_relative_to_nadir\n" + "5.0,-40.0,10,-0.8\n" + "this,is,a,bad,row\n" // 4th field "bad" -> stof throws -> skipped + "1.0,-40.0,10,0.0\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[0].first, 1.0f); // sorted ascending + EXPECT_FLOAT_EQ(curve[1].first, 5.0f); +} + +// A missing/empty path yields an empty curve (the correction degrades to no-op). +TEST(AngularResponseCurve, MissingFileIsEmpty) +{ + EXPECT_TRUE(loadAngularResponseCurve("").empty()); + EXPECT_TRUE(loadAngularResponseCurve("/no/such/curve_file_12345.csv").empty()); +} + +// Mode-string parsing is case-insensitive; unknown values are rejected. +TEST(AngularResponseCurve, ParsesMode) +{ + BackscatterAngleCorrection mode = BackscatterAngleCorrection::Empirical; + ASSERT_TRUE(parseBackscatterAngleCorrection("none", mode)); + EXPECT_EQ(mode, BackscatterAngleCorrection::None); + + ASSERT_TRUE(parseBackscatterAngleCorrection("Empirical", mode)); + EXPECT_EQ(mode, BackscatterAngleCorrection::Empirical); + + ASSERT_TRUE(parseBackscatterAngleCorrection("EMPIRICAL", mode)); + EXPECT_EQ(mode, BackscatterAngleCorrection::Empirical); + + // Unknown value: rejected, out param left unchanged. + mode = BackscatterAngleCorrection::Empirical; + EXPECT_FALSE(parseBackscatterAngleCorrection("bogus", mode)); + EXPECT_EQ(mode, BackscatterAngleCorrection::Empirical); +} + +} // namespace cube diff --git a/cube_bathymetry/test/test_hypothesis.cpp b/cube_bathymetry/test/test_hypothesis.cpp index c1dffce..5603bad 100644 --- a/cube_bathymetry/test/test_hypothesis.cpp +++ b/cube_bathymetry/test/test_hypothesis.cpp @@ -213,9 +213,9 @@ TEST_F(HypothesisTest, RecordBeamAppendsRawAndAngle) ASSERT_EQ(h.intensity_samples.size(), 2u); EXPECT_FLOAT_EQ(h.intensity_samples[0].raw_intensity, -30.0f); - EXPECT_FLOAT_EQ(h.intensity_samples[0].grazing_angle, 0.1f); + EXPECT_FLOAT_EQ(h.intensity_samples[0].beam_angle, 0.1f); EXPECT_FLOAT_EQ(h.intensity_samples[1].raw_intensity, -28.0f); - EXPECT_FLOAT_EQ(h.intensity_samples[1].grazing_angle, 0.2f); + EXPECT_FLOAT_EQ(h.intensity_samples[1].beam_angle, 0.2f); } // A NaN raw intensity (source omitted intensities) must never become a phantom @@ -231,7 +231,7 @@ TEST_F(HypothesisTest, RecordBeamSkipsNanIntensity) EXPECT_EQ(h.intensity_samples.size(), 1u); } -// A NaN grazing angle is retained: the beam is still a valid intensity sample, +// A NaN beam angle is retained: the beam is still a valid intensity sample, // it simply cannot be angle-corrected (emitted uncorrected at output). TEST_F(HypothesisTest, RecordBeamRetainsNanAngle) { @@ -240,7 +240,7 @@ TEST_F(HypothesisTest, RecordBeamRetainsNanAngle) h.recordBeam(-20.0f, std::nan("")); ASSERT_EQ(h.intensity_samples.size(), 1u); EXPECT_FLOAT_EQ(h.intensity_samples[0].raw_intensity, -20.0f); - EXPECT_TRUE(std::isnan(h.intensity_samples[0].grazing_angle)); + EXPECT_TRUE(std::isnan(h.intensity_samples[0].beam_angle)); } } // namespace cube diff --git a/cube_bathymetry/test/test_node.cpp b/cube_bathymetry/test/test_node.cpp index 4f60deb..56fea50 100644 --- a/cube_bathymetry/test/test_node.cpp +++ b/cube_bathymetry/test/test_node.cpp @@ -570,4 +570,118 @@ TEST_F(NodeTest, IntensityVarNonNegativeAfterClamp) EXPECT_GE(record.intensity_var, 0.0f); } +// ---- Empirical angular-response (ARA) correction (#81) --------------------- + +namespace +{ +// Degrees -> radians, matching how rx_angles are stored on a sample (radians). +float deg2rad(float deg) {return deg * static_cast(M_PI) / 180.0f;} + +// A Parameters with the Empirical ARA mode + a known 2-point curve: +// nadir bin (0 deg -> 0 dB) and edge bin (60 deg -> -12 dB). At 30 deg the +// linearly-interpolated db_relative_to_nadir is -6 dB. +Parameters empiricalParams() +{ + Parameters p{CellSizes(1.0f), "order1a"}; + p.backscatter_angle_correction = BackscatterAngleCorrection::Empirical; + p.angular_response_curve = {{0.0f, 0.0f}, {60.0f, -12.0f}}; + return p; +} + +// Build a single-beam node via a nominated hypothesis so the surfaced intensity +// equals the corrected value of exactly that beam (no averaging). +std::shared_ptr singleBeamNominated( + Node & n, float raw_intensity, float beam_angle_rad) +{ + auto h = std::make_shared(10.0f, 1.0f); + h->input_sample_variance = 1.0f; + h->recordBeam(raw_intensity, beam_angle_rad); + NodeNominationTestAccess::nominate(n, h); + return h; +} +} // namespace + +// Mid-angle beam: corrected = raw - curveRel(30 deg) = -30 - (-6) = -24. +TEST_F(NodeTest, ARAInterpolation) +{ + Parameters ara = empiricalParams(); + Node n; + singleBeamNominated(n, -30.0f, deg2rad(30.0f)); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -24.0f, 1e-3); +} + +// Nadir beam: curveRel(0) = 0 -> corrected == raw. +TEST_F(NodeTest, ARANadirIdentity) +{ + Parameters ara = empiricalParams(); + Node n; + singleBeamNominated(n, -30.0f, 0.0f); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -30.0f, 1e-4); +} + +// Beyond the curve's max angle (60 deg): identity (no extrapolation). +TEST_F(NodeTest, ARABeyondMaxAngleClampsToEdge) +{ + // Beyond the curve's last bin (60 deg) the correction clamps to the outermost + // bin value (-12 dB), NOT identity -- continuous, no swath-edge jump (#81 review). + // corrected = raw - curveRel = -30 - (-12) = -18. + Parameters ara = empiricalParams(); + Node n; + singleBeamNominated(n, -30.0f, deg2rad(70.0f)); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -18.0f, 1e-3); +} + +// NaN beam angle: no correction applicable -> identity. +TEST_F(NodeTest, ARANaNAngleIdentity) +{ + Parameters ara = empiricalParams(); + Node n; + singleBeamNominated(n, -30.0f, std::nan("")); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -30.0f, 1e-4); +} + +// Mode None on a non-nadir beam: the correction never runs -> corrected == raw. +TEST_F(NodeTest, ARAOffIsIdentity) +{ + Node n; + singleBeamNominated(n, -30.0f, deg2rad(30.0f)); + + auto record = n.extractNodeRecord(params); // default None + ASSERT_EQ(record.n_samples, 1u); + EXPECT_NEAR(record.intensity, -30.0f, 1e-4); +} + +// Port/starboard symmetry: +30 deg and -30 deg get the same correction because +// the curve is keyed on |beam_angle|. Two equal-magnitude beams -> equal +// corrected values -> mean -24, zero spread. +TEST_F(NodeTest, ARAPortStarboardSymmetry) +{ + Parameters ara = empiricalParams(); + Node n; + auto h = std::make_shared(10.0f, 1.0f); + h->input_sample_variance = 1.0f; + h->recordBeam(-30.0f, deg2rad(30.0f)); // starboard + h->recordBeam(-30.0f, deg2rad(-30.0f)); // port + NodeNominationTestAccess::nominate(n, h); + + auto record = n.extractNodeRecord(ara); + ASSERT_EQ(record.n_samples, 2u); + EXPECT_NEAR(record.intensity, -24.0f, 1e-3); + // Equal corrected values -> zero estimate variance (clamped). + ASSERT_FALSE(std::isnan(record.intensity_var)); + EXPECT_NEAR(record.intensity_var, 0.0f, 1e-4); +} + } // namespace cube diff --git a/cube_bathymetry/test/test_store_import.cpp b/cube_bathymetry/test/test_store_import.cpp index 5305e13..1e0c160 100644 --- a/cube_bathymetry/test/test_store_import.cpp +++ b/cube_bathymetry/test/test_store_import.cpp @@ -63,8 +63,9 @@ std::vector makeSoundings() // Same synthetic soundings as makeSoundings(), but each carries a constant // backscatter intensity so the CUBE node co-estimates a finite per-cell // intensity (the offline backscatter surface, #80). beam_angle is carried too -// (it rides the {intensity, angle} pair), though the surfaced value is -// uncorrected so the angle does not change it here. +// (it rides the {intensity, angle} pair), though by default +// (BackscatterAngleCorrection::None) the surfaced value is uncorrected so the +// angle does not change it here. std::vector makeSoundingsWithIntensity(float intensity) { std::vector soundings = makeSoundings(); @@ -268,7 +269,8 @@ TEST(StoreImport, LoadIntoSheetEmptyLayerIsNoOp) // Intensity-bearing soundings must produce a non-empty backscatter cell map whose // cells match the grid's finite-intensity nodeRecords() entries cell-for-cell, // carry the import timestamp/source, and (with a constant input intensity) surface -// that same constant value (the surfaced backscatter is uncorrected, #80). +// that same constant value (the surfaced backscatter is uncorrected by default, +// BackscatterAngleCorrection::None, #80). TEST(StoreImport, BackscatterCellsMatchGridRecords) { GeoMapSheet ms(1.0f); @@ -300,8 +302,8 @@ TEST(StoreImport, BackscatterCellsMatchGridRecords) ++finite_total; ASSERT_NE(found, cells.end()); EXPECT_FLOAT_EQ(found->second.intensity, records[k].intensity); - // Constant input intensity, uncorrected surface -> emitted value is that - // constant (mean of equal per-beam intensities). + // Constant input intensity, uncorrected surface by default (None) -> + // emitted value is that constant (mean of equal per-beam intensities). EXPECT_FLOAT_EQ(found->second.intensity, kIntensity); EXPECT_EQ(found->second.timestamp, kStamp); EXPECT_EQ(found->second.source_index, kSource);