From 3f22a8516e4893fda73756b43793be17341d3115 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 09:56:09 +0000 Subject: [PATCH 01/17] progress: issue review for #81 --- .agent/work-plans/issue-81/progress.md | 117 +++++++++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 .agent/work-plans/issue-81/progress.md diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md new file mode 100644 index 0000000..8505707 --- /dev/null +++ b/.agent/work-plans/issue-81/progress.md @@ -0,0 +1,117 @@ +--- +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). From 94938a6d0392a35435a84a889331a957aaf5feb0 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 10:29:35 +0000 Subject: [PATCH 02/17] Add work plan for #81 Replace Phase B no-op with first-cut flat-bottom Lambert correction using 10*log10(cos(|beam_angle|)); rename misnamed grazing_angle field; add ADR-0007 transition note; update affected tests. --- .../.agent/work-plans/issue-81/plan.md | 158 ++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 cube_bathymetry/.agent/work-plans/issue-81/plan.md diff --git a/cube_bathymetry/.agent/work-plans/issue-81/plan.md b/cube_bathymetry/.agent/work-plans/issue-81/plan.md new file mode 100644 index 0000000..7c777ea --- /dev/null +++ b/cube_bathymetry/.agent/work-plans/issue-81/plan.md @@ -0,0 +1,158 @@ +# Plan: Apply deferred M3 backscatter angle-correction in shared CUBE estimator + +## Issue + +https://github.com/rolker/cube_bathymetry/issues/81 + +## Context + +`Node::extractNodeRecord()` (node.cpp:315-318) contains a Phase B no-op where +`corrected == raw` for every beam. The per-beam `{raw_intensity, grazing_angle}` +pairs are already stored on each hypothesis (`Hypothesis::intensity_samples`). +The gate — verifying `rx_angles` sign/zero convention against the M3 producer — +has been confirmed host-side: `kongsberg_em_bridge` negates the Kongsberg raw +pointing angle (`+PORT → -PORT`) so `rx_angles` is `+STARBOARD`, nadir=0, +radians; intensity is `reflectivity_db` in dB. Gate: **PASS**. + +The `BeamIntensitySample::grazing_angle` field (hypothesis.h:49) is misnamed: +it stores the receive steering / incidence angle from nadir (same semantics as +`sounding.beam_angle`), NOT a true grazing angle (which would be 90° − θ). The +field doc already says "receive/steering (beam/incidence) angle" so the comment +is correct but the name is wrong. Plan renames it to `beam_angle` for consistency. + +ADR-0007 has no document file in `docs/decisions/` yet; this PR adds the Phase B +transition note and files a follow-up task for the full ADR-0007 doc. + +## Lambert Formula Derivation + +`raw_intensity` is M3 `reflectivity_db` (dB), already TVG-compensated. The +remaining angular dependence is the receive-beam Lambert cosine: + +``` +BS(θ) = BS(0) + 10·log10(cos θ) [one-way, TVG already handled] +``` + +where θ = incidence from nadir = `std::abs(sample.beam_angle)`. + +To normalize to nadir (remove angular dependence): + +``` +corrected_dB = raw_dB − 10·log10(cos θ) +``` + +**Derivation rationale:** +- One-way (10·, not 20·): the transmit spreading loss is already in TVG; only + the receive-beam incidence factor is uncompensated here. +- `cos θ` not `cos²θ`: cos²θ applies when both transmit and receive solid angles + depend on θ; here only the receive steering angle is being corrected. +- At nadir (θ=0): `cos(0)=1`, `log10(1)=0` → correction=0 (identity). ✓ +- At θ>0: `cos θ<1`, `log10(cos θ)<0` → correction is positive in dB (boosts + off-nadir returns toward their nadir-equivalent). Physically correct for + Lambert scattering where off-nadir returns are weaker. +- `cos θ → 0` as θ→90° guard: clamp `θ` to a maximum of `MAX_INCIDENCE_RAD = + 80° = 1.3963 rad` before computing. Beams beyond this are emitted uncorrected + (identity) — the correction diverges and the Lambert model fails at near-grazing. +- NaN `beam_angle` → emit uncorrected (identity). Consistent with how `recordBeam` + already retains NaN-angle beams as "no angle correction available". + +**This formula is the highest-risk decision in this issue.** The review-plan step +must scrutinize it. Prior art: the sibling slope-correction (#15) had 2 wrong +formulas killed across 3 rounds. + +## Approach + +1. **Rename `BeamIntensitySample::grazing_angle` → `beam_angle`** — update the + struct (hypothesis.h:49), `recordBeam` signature/impl (hypothesis.h:118, + hypothesis.cpp:118,122,127), all call sites (node.cpp:308 comment), and all + test references (test_hypothesis.cpp:216,218,227,230,243). Corrects the + misleading name and aligns with `sounding.beam_angle`. + +2. **Replace the Phase B no-op in `node.cpp:315-318`** — replace + `const double corrected = sample.raw_intensity;` with the flat-bottom Lambert + correction using `std::abs(sample.beam_angle)`. Add the inline comment + capturing the rx_angles producer cross-check (Kongsberg +PORT→negate→+STARBOARD; + nadir=0; radians; dB). Update/remove the stale `TODO(#54-B / #15)` block at + node.cpp:298-312 to reflect Phase B completion; retain the follow-up note + for the full GeoCoder correction (#15). + +3. **Update `test_node.cpp` for corrected intensity values** — existing tests that + pass `beam_angle = 0.0f` to `recordBeam` (or use nadir beams) continue to + satisfy `corrected == raw` (identity at θ=0). Tests that use non-zero + `beam_angle` (e.g., `NodeRecordMeanAndEstimateVariance` uses 0.1 rad beams) + must now expect Lambert-corrected values. Add: + - `LambertCorrectionNonNadir`: a non-nadir beam (e.g. θ=0.5 rad) produces + `corrected ≠ raw`, and specifically `corrected = raw − 10·log10(cos(0.5))`. + - `LambertCorrectionNadirIdentity`: θ≈0 → `corrected == raw` within float tol. + - `LambertCorrectionPortStarboardSymmetry`: equal `|beam_angle|` on each side + (e.g., +0.3 and −0.3 rad) produces equal corrected intensity. + - `LambertCorrectionNaNAngle`: NaN `beam_angle` → identity (uncorrected). + - `LambertCorrectionNearGrazing`: θ ≥ MAX_INCIDENCE_RAD → identity (clamped). + +4. **Check `test_store_import.cpp`** — `BackscatterCellsMatchGridRecords` uses + `kIntensity = 42.5f` with `beam_angle = 0.1f`. After the correction, the + surfaced value will be `42.5 − 10·log10(cos(0.1)) ≈ 42.5 + 0.022 ≈ 42.52`, + no longer exactly `kIntensity`. Fix: either change `beam_angle` to 0.0f in + `makeSoundingsWithIntensity` (so nadir beams keep the identity), or update the + assertion to expect the corrected value. Using 0.0f is simpler and keeps the + test self-consistent. + +5. **Add ADR-0007 transition note** — create + `docs/decisions/0007-mbes-backscatter-store-phase-b-transition.md` containing: + a minimal transition note recording: Phase B ended in this PR, first-cut + flat-bottom Lambert applied at `extractNodeRecord()`, what stays deferred (full + GeoCoder incidence via #15 + #59). File a separate GitHub issue to author the + full ADR-0007 document. + +## Files to Change + +| File | Change | +|------|--------| +| `include/cube_bathymetry/hypothesis.h` | Rename `grazing_angle` → `beam_angle` in struct and `recordBeam` decl | +| `src/hypothesis.cpp` | Rename `grazing_angle` param in `recordBeam` impl | +| `src/node.cpp` | Replace no-op with Lambert correction; add producer cross-check comment; update TODO block | +| `test/test_node.cpp` | Update non-nadir intensity assertions; add 5 Lambert tests | +| `test/test_hypothesis.cpp` | Update `grazing_angle` field refs → `beam_angle` | +| `test/test_store_import.cpp` | Fix `makeSoundingsWithIntensity` to use `beam_angle = 0.0f` (nadir) | +| `docs/decisions/0007-mbes-backscatter-store-phase-b-transition.md` | New: transition note | + +## Principles Self-Check + +| Principle | Consideration | +|---|---| +| Enforcement over documentation | Add 5 targeted tests that would catch the wrong formula, wrong sign, and missing NaN/grazing guards — enforcement via test, not just comments | +| Capture decisions | ADR-0007 transition note records Phase B end date, formula chosen, what's deferred | +| A change includes its consequences | `test_store_import.cpp` fix included in same PR; both live (#78) and offline (#80) paths automatically corrected via the shared `extractNodeRecord()` | +| Only what's needed | Full GeoCoder correction (#15, #59) explicitly deferred; no per-path duplication | +| Improve incrementally | Phase B → first-cut Lambert → full GeoCoder (#15) — keeps each step reviewable | + +## ADR Compliance + +| ADR | Triggered | How addressed | +|---|---|---| +| ADR-0007 D3 (incidence correction at node-output) | Yes — primary | Implemented here at `extractNodeRecord()` | +| ADR-0007 D2/D4 (per-beam mean + estimate variance) | Yes — already in place | No change; `corrected` replaces `raw_intensity` in the sum | +| ADR-0001 (adopt ADRs) | Yes | Transition note in `docs/decisions/` | +| ADR-0013 (progress.md vocabulary) | Yes | `## Plan Authored` entry committed with plan | + +## Consequences + +| If we change... | Also update... | Included in plan? | +|---|---|---| +| `BeamIntensitySample::grazing_angle` name | `hypothesis.cpp`, `test_hypothesis.cpp`, `node.cpp` comment | Yes — Step 1 | +| `corrected` value in `extractNodeRecord()` | `test_node.cpp` non-nadir assertions | Yes — Step 3 | +| `corrected` value flows to `test_store_import.cpp` via `nodeRecords()` | `makeSoundingsWithIntensity` beam_angle=0.1 assertion | Yes — Step 4 | +| Phase B ends | ADR-0007 transition note | Yes — Step 5 | +| Full GeoCoder correction | cube#15, cube#59 | No — explicit follow-up | + +## Open Questions + +- [ ] Formula constant: this plan uses 10·log10(cos θ) (one-way Lambert, TVG + handled). If the M3 `reflectivity_db` already includes the Lambert receive + factor in its TVG (i.e., TVG corrects for cos θ), the correction double-counts. + Confirm from Kongsberg EM Datagram Formats that `reflectivity_db` is raw + backscatter without Lambert normalization before implementing. + +## Estimated Scope + +Single PR. All changes touch one package (`cube_bathymetry`). ~120 lines changed, +~80 new test lines. From dbc28491fa219996f423e2ad42dd849bc3b6ffb6 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 10:29:57 +0000 Subject: [PATCH 03/17] progress: plan authored for #81 --- .agent/work-plans/issue-81/progress.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index 8505707..784dd7f 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -115,3 +115,15 @@ implementation and test. - [ ] 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. From 9efe563dfc11bd5bc749b5e9cae305cce0f81bbf Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 06:46:02 -0400 Subject: [PATCH 04/17] chore: move #81 plan.md to repo-root work-plan dir (beside progress.md) plan-task wrote it under the package subdir; normalize to the repo-root .agent/work-plans/issue-81/ location (matching progress.md and the #80 convention) so review-plan and future agents find it. Part of #81 --- {cube_bathymetry/.agent => .agent}/work-plans/issue-81/plan.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {cube_bathymetry/.agent => .agent}/work-plans/issue-81/plan.md (100%) diff --git a/cube_bathymetry/.agent/work-plans/issue-81/plan.md b/.agent/work-plans/issue-81/plan.md similarity index 100% rename from cube_bathymetry/.agent/work-plans/issue-81/plan.md rename to .agent/work-plans/issue-81/plan.md From 71402e7ea941fef97f27bd62995e07caf3b02b43 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 10:51:04 +0000 Subject: [PATCH 05/17] progress: plan review for #81 --- .agent/work-plans/issue-81/progress.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index 784dd7f..3f88dea 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -127,3 +127,23 @@ implementation and test. ### 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` From 0387a2447a98cd2047c8a9306b2f89306a4eda30 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 07:17:43 -0400 Subject: [PATCH 06/17] plan(#81): revise per plan-review + operator decisions Reshape after review-plan (changes-requested) + operator direction: - Sonar-agnostic: the angle-correction becomes an operator startup parameter (BackscatterAngleCorrection None|Lambert, default None=identity) on both estimator entry points (live node ROS param + import_bag CLI) -- the estimator cannot know a sonar's internal normalization, so the operator declares it per sonar. Verified host-side that kongsberg_em_bridge reads raw EM 'Raw Range and Angle 78' reflectivity with no bridge-side angle compensation. - Formula: cos^2 theta => corrected_dB = raw_dB - 20*log10(cos theta) (classic Lambert; operator-confirmed; the prior x10 justification was physically unsound per review). - Near-grazing: single rule -- identity beyond MAX_INCIDENCE (no clamp). - Tests: enumerate all 3 broken non-nadir assertions; add a default-off identity test; #80 offline test stays valid on the default path. - ADR-0007 transition note renamed to an addendum (no 0007 slot collision) + follow-up to author the full ADR. Part of #81 --- .agent/work-plans/issue-81/plan.md | 268 +++++++++++++++++------------ 1 file changed, 158 insertions(+), 110 deletions(-) diff --git a/.agent/work-plans/issue-81/plan.md b/.agent/work-plans/issue-81/plan.md index 7c777ea..59c6bb2 100644 --- a/.agent/work-plans/issue-81/plan.md +++ b/.agent/work-plans/issue-81/plan.md @@ -7,152 +7,200 @@ https://github.com/rolker/cube_bathymetry/issues/81 ## Context `Node::extractNodeRecord()` (node.cpp:315-318) contains a Phase B no-op where -`corrected == raw` for every beam. The per-beam `{raw_intensity, grazing_angle}` +`corrected == raw` for every beam. The per-beam `{raw_intensity, beam_angle}` pairs are already stored on each hypothesis (`Hypothesis::intensity_samples`). -The gate — verifying `rx_angles` sign/zero convention against the M3 producer — -has been confirmed host-side: `kongsberg_em_bridge` negates the Kongsberg raw -pointing angle (`+PORT → -PORT`) so `rx_angles` is `+STARBOARD`, nadir=0, -radians; intensity is `reflectivity_db` in dB. Gate: **PASS**. -The `BeamIntensitySample::grazing_angle` field (hypothesis.h:49) is misnamed: +**Gate (rx_angles sign/zero convention) — VERIFIED host-side, PASS.** +`kongsberg_em_bridge/node.py:364-373` negates the Kongsberg raw pointing angle +(`+PORT → +STARBOARD`, matching `marine_acoustic_msgs`); nadir = 0 (boresight; +physical mount is in TF), radians. For a flat-bottom Lambert using `|angle|` the +sign is moot (cos is even); nadir=0 and `|angle|`=incidence-from-nadir both hold. + +**Is `reflectivity_db` already angle-normalized? — operator-controlled, NOT +hardcoded (operator decision).** `em_datagrams.py:120-126` reads `reflectivity_db` +straight from the EM **"Raw Range and Angle 78"** datagram (signed 16-bit ×0.1 dB) +with **no angle compensation in the bridge**. Whether the *sonar firmware* +pre-normalized that reflectivity for incidence is sonar/config-dependent and is +NOT determinable from workspace code. **This estimator is sonar-agnostic** (it +also runs Norbit, R2Sonic, sim, …), so whether to apply the Lambert correction +**must be an operator-set startup parameter**, not an M3 assumption baked into the +shared estimator. Default is **off (identity = current Phase B behavior)** — safe +and sonar-agnostic; a platform/config enables `lambert` for a sonar known to +deliver raw (non-angle-normalized) backscatter. + +The `BeamIntensitySample::grazing_angle` field (hypothesis.h:49) is **misnamed**: it stores the receive steering / incidence angle from nadir (same semantics as -`sounding.beam_angle`), NOT a true grazing angle (which would be 90° − θ). The -field doc already says "receive/steering (beam/incidence) angle" so the comment -is correct but the name is wrong. Plan renames it to `beam_angle` for consistency. +`sounding.beam_angle`), NOT a true grazing angle (90° − θ). The field doc already +says "receive/steering (beam/incidence) angle", so the comment is correct but the +name is wrong. Plan renames it to `beam_angle`. -ADR-0007 has no document file in `docs/decisions/` yet; this PR adds the Phase B -transition note and files a follow-up task for the full ADR-0007 doc. +ADR-0007 has no document file in `docs/decisions/` yet; this PR adds a Phase B +transition **addendum** (not claiming the `0007-` slot) and files a follow-up +task for the full ADR-0007 doc. -## Lambert Formula Derivation +## Lambert Formula (operator-confirmed cos²θ) -`raw_intensity` is M3 `reflectivity_db` (dB), already TVG-compensated. The -remaining angular dependence is the receive-beam Lambert cosine: +`raw_intensity` is `reflectivity_db` (dB). The angular correction is **additive in +dB** and applied ONLY when the operator parameter selects `lambert`: ``` -BS(θ) = BS(0) + 10·log10(cos θ) [one-way, TVG already handled] +corrected_dB = raw_dB − 20·log10(cos θ) θ = std::abs(sample.beam_angle) ``` -where θ = incidence from nadir = `std::abs(sample.beam_angle)`. - -To normalize to nadir (remove angular dependence): - -``` -corrected_dB = raw_dB − 10·log10(cos θ) -``` - -**Derivation rationale:** -- One-way (10·, not 20·): the transmit spreading loss is already in TVG; only - the receive-beam incidence factor is uncompensated here. -- `cos θ` not `cos²θ`: cos²θ applies when both transmit and receive solid angles - depend on θ; here only the receive steering angle is being corrected. -- At nadir (θ=0): `cos(0)=1`, `log10(1)=0` → correction=0 (identity). ✓ -- At θ>0: `cos θ<1`, `log10(cos θ)<0` → correction is positive in dB (boosts - off-nadir returns toward their nadir-equivalent). Physically correct for - Lambert scattering where off-nadir returns are weaker. -- `cos θ → 0` as θ→90° guard: clamp `θ` to a maximum of `MAX_INCIDENCE_RAD = - 80° = 1.3963 rad` before computing. Beams beyond this are emitted uncorrected - (identity) — the correction diverges and the Lambert model fails at near-grazing. -- NaN `beam_angle` → emit uncorrected (identity). Consistent with how `recordBeam` - already retains NaN-angle beams as "no angle correction available". - -**This formula is the highest-risk decision in this issue.** The review-plan step -must scrutinize it. Prior art: the sibling slope-correction (#15) had 2 wrong -formulas killed across 3 rounds. +**Form rationale (cos²θ / factor 20 — the classic seafloor Lambert law, +operator-confirmed; the reviewer flagged the earlier ×10 justification as +physically unsound):** +- Lambert backscatter cross-section per unit area `σ(θ) ∝ cos²θ` — a single + seabed-scattering term (insonified-footprint projection cosine × Lambertian + re-emission cosine), NOT a transmit/receive split. In dB: + `BS(θ) = BS(0) + 20·log10(cos θ)`. TVG handles range spreading/absorption, not + this angular factor — so it is not already removed. +- Normalizing to the nadir-equivalent ⇒ subtract the angular term: + `corrected_dB = raw_dB − 20·log10(cos θ)`. +- Nadir (θ=0): `cos0=1`, `log10(1)=0` → correction 0 (identity). ✓ +- Off-nadir (θ>0): `cos θ<1` → `−20·log10(cos θ) > 0` → boosts the weaker + off-nadir return toward its nadir-equivalent. + +**Near-grazing handling — single unambiguous rule (resolves the prior +contradiction):** beams with `θ ≥ MAX_INCIDENCE_RAD` (= 80° = 1.39626 rad) are +emitted **UNCORRECTED (identity)** — there is **no θ-clamping**. Rationale: the +Lambert model fails and `−20·log10(cos θ)` diverges near grazing; a clamp would +emit a physically meaningless frozen-at-80° value. Identity-beyond-max is the one +behavior; the test asserts identity beyond the cap. (The step at the cap is +accepted: those beams are unreliable either way.) +- NaN `beam_angle` → identity (consistent with `recordBeam` retaining NaN-angle + beams as "no angle correction available"). + +**This formula was the highest-risk decision; it has been operator-confirmed +(cos²θ) and independently flagged by review-plan. Re-review the revised plan +before implementing.** ## Approach -1. **Rename `BeamIntensitySample::grazing_angle` → `beam_angle`** — update the - struct (hypothesis.h:49), `recordBeam` signature/impl (hypothesis.h:118, - hypothesis.cpp:118,122,127), all call sites (node.cpp:308 comment), and all - test references (test_hypothesis.cpp:216,218,227,230,243). Corrects the - misleading name and aligns with `sounding.beam_angle`. - -2. **Replace the Phase B no-op in `node.cpp:315-318`** — replace - `const double corrected = sample.raw_intensity;` with the flat-bottom Lambert - correction using `std::abs(sample.beam_angle)`. Add the inline comment - capturing the rx_angles producer cross-check (Kongsberg +PORT→negate→+STARBOARD; - nadir=0; radians; dB). Update/remove the stale `TODO(#54-B / #15)` block at - node.cpp:298-312 to reflect Phase B completion; retain the follow-up note - for the full GeoCoder correction (#15). - -3. **Update `test_node.cpp` for corrected intensity values** — existing tests that - pass `beam_angle = 0.0f` to `recordBeam` (or use nadir beams) continue to - satisfy `corrected == raw` (identity at θ=0). Tests that use non-zero - `beam_angle` (e.g., `NodeRecordMeanAndEstimateVariance` uses 0.1 rad beams) - must now expect Lambert-corrected values. Add: - - `LambertCorrectionNonNadir`: a non-nadir beam (e.g. θ=0.5 rad) produces - `corrected ≠ raw`, and specifically `corrected = raw − 10·log10(cos(0.5))`. - - `LambertCorrectionNadirIdentity`: θ≈0 → `corrected == raw` within float tol. - - `LambertCorrectionPortStarboardSymmetry`: equal `|beam_angle|` on each side - (e.g., +0.3 and −0.3 rad) produces equal corrected intensity. - - `LambertCorrectionNaNAngle`: NaN `beam_angle` → identity (uncorrected). - - `LambertCorrectionNearGrazing`: θ ≥ MAX_INCIDENCE_RAD → identity (clamped). - -4. **Check `test_store_import.cpp`** — `BackscatterCellsMatchGridRecords` uses - `kIntensity = 42.5f` with `beam_angle = 0.1f`. After the correction, the - surfaced value will be `42.5 − 10·log10(cos(0.1)) ≈ 42.5 + 0.022 ≈ 42.52`, - no longer exactly `kIntensity`. Fix: either change `beam_angle` to 0.0f in - `makeSoundingsWithIntensity` (so nadir beams keep the identity), or update the - assertion to expect the corrected value. Using 0.0f is simpler and keeps the - test self-consistent. - -5. **Add ADR-0007 transition note** — create - `docs/decisions/0007-mbes-backscatter-store-phase-b-transition.md` containing: - a minimal transition note recording: Phase B ended in this PR, first-cut - flat-bottom Lambert applied at `extractNodeRecord()`, what stays deferred (full - GeoCoder incidence via #15 + #59). File a separate GitHub issue to author the - full ADR-0007 document. +0. **Add the sonar-agnostic correction parameter** to `Parameters` + (`parameters.h`): an enum + `enum class BackscatterAngleCorrection { None, Lambert };` and a field + `BackscatterAngleCorrection backscatter_angle_correction = BackscatterAngleCorrection::None;` + (default None = identity = current behavior). Plumb it as a startup setting on + both estimator entry points: + - **Live node** (`cube_bathymetry_node.cpp`): declare a ROS parameter + `backscatter_angle_correction` (string `"none"`|`"lambert"`, default + `"none"`), parse into the enum when building `Parameters`. + - **Offline import** (`import_bag_main.cpp`): a `--backscatter-correction + none|lambert` flag (default `none`), documented in `usage()`. + Both run the same estimator, so one parameter corrects both the live (#78) and + offline (#80) paths consistently when enabled. + +1. **Rename `BeamIntensitySample::grazing_angle` → `beam_angle`** — struct + + field doc (`hypothesis.h:40-50`, **incl. the doc text at `hypothesis.h:115-116` + and the `BeamIntensitySample` reference at `hypothesis.h:166`**), `recordBeam` + signature/impl (`hypothesis.h:118`, `hypothesis.cpp:118,122,127`), the + `node.cpp` call-site comment, and test references + (`test_hypothesis.cpp:216,218,227,230,243`). + +2. **Replace the Phase B no-op in `node.cpp:315-318`** — when + `parameters.backscatter_angle_correction == Lambert`, set + `corrected = raw − 20·log10(cos(std::abs(beam_angle)))` with the near-grazing / + NaN identity rules above; otherwise `corrected = raw` (identity). Add the inline + comment capturing the producer cross-check (Kongsberg +PORT→negate→+STARBOARD; + nadir=0; radians; dB) AND that the correction is operator-gated because the + sonar-internal normalization state is not knowable here. Update the stale + `TODO(#54-B / #15)` block (`node.cpp:298-312`): Phase B done for the flat-bottom + case; retain the follow-up note for the full GeoCoder slope-aware correction + (#15/#59). + +3. **Update `test_node.cpp`** — enumerate **all three** non-nadir intensity + assertions that break under `lambert` (the new tests run with the param ON): + - `NodeRecordMeanAndEstimateVariance` — the **mean** assertion changes; the + **variance** assertion still holds (all four beams share one angle, so the + per-beam offset is constant and cancels in the variance). + - `FirstBeamInitializationRecordsIntensity` (`test_node.cpp:382`, + `EXPECT_FLOAT_EQ(record.intensity, -30.0f)`). + - `NodeRecordSkipsNanIntensityBeam` (`test_node.cpp:420`). + These existing tests stay on the **default (None)** path unless they opt into + `lambert`; assertions that exercise the correction must set the param ON. + Add Lambert tests (param = Lambert): + - `LambertCorrectionNonNadir`: θ=0.5 rad ⇒ `corrected = raw − 20·log10(cos 0.5)`, + `corrected ≠ raw`. + - `LambertCorrectionNadirIdentity`: θ≈0 ⇒ `corrected == raw` (float tol). + - `LambertCorrectionPortStarboardSymmetry`: ±0.3 rad ⇒ equal corrected value. + - `LambertCorrectionNaNAngle`: NaN angle ⇒ identity. + - `LambertCorrectionNearGrazing`: θ ≥ MAX_INCIDENCE_RAD ⇒ identity. + - `CorrectionOffIsIdentity`: param = None ⇒ `corrected == raw` for a non-nadir + beam (guards the default path). + +4. **`test_store_import.cpp` (#80 backscatter test) stays valid by default** — + the offline tests build `Parameters` with the default (None), so + `BackscatterCellsMatchGridRecords` (uses `beam_angle = 0.1f`, asserts surfaced + value == `kIntensity`) **still holds unchanged** (no correction by default). + **Update the stale comment** at `test_store_import.cpp:64-67` ("the surfaced + value is uncorrected so the angle does not change it here") to say *by default* + (None); optionally add a focused case that sets the param to Lambert and + asserts the corrected value flows through the offline path. (Preferred over + zeroing `beam_angle`, per review-plan — keeps a non-nadir beam exercising it.) + +5. **Add ADR-0007 transition addendum** — create + `docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md` + (sibling **addendum** name — does NOT claim the canonical `0007-…` slot before + the full ADR exists): records Phase B end, the operator-gated flat-bottom + Lambert (cos²θ), the sonar-agnostic parameter + default-off rationale, and what + stays deferred (full GeoCoder incidence via #15/#59). **File a separate GitHub + issue** to author the full ADR-0007 document. ## Files to Change | File | Change | |------|--------| -| `include/cube_bathymetry/hypothesis.h` | Rename `grazing_angle` → `beam_angle` in struct and `recordBeam` decl | -| `src/hypothesis.cpp` | Rename `grazing_angle` param in `recordBeam` impl | -| `src/node.cpp` | Replace no-op with Lambert correction; add producer cross-check comment; update TODO block | -| `test/test_node.cpp` | Update non-nadir intensity assertions; add 5 Lambert tests | -| `test/test_hypothesis.cpp` | Update `grazing_angle` field refs → `beam_angle` | -| `test/test_store_import.cpp` | Fix `makeSoundingsWithIntensity` to use `beam_angle = 0.0f` (nadir) | -| `docs/decisions/0007-mbes-backscatter-store-phase-b-transition.md` | New: transition note | +| `include/cube_bathymetry/parameters.h` | Add `BackscatterAngleCorrection` enum + `backscatter_angle_correction` field (default None) | +| `include/cube_bathymetry/hypothesis.h` | Rename `grazing_angle` → `beam_angle` (struct + `recordBeam` decl + doc comments at 115-116, 166) | +| `src/hypothesis.cpp` | Rename param in `recordBeam` impl | +| `src/node.cpp` | Param-gated cos²θ Lambert correction; producer cross-check comment; update TODO block | +| `src/cube_bathymetry_node.cpp` | Declare/parse `backscatter_angle_correction` ROS param → Parameters | +| `src/import_bag_main.cpp` | `--backscatter-correction none|lambert` flag + usage() text → Parameters | +| `test/test_node.cpp` | Fix 3 broken non-nadir assertions; add 6 Lambert/param tests | +| `test/test_hypothesis.cpp` | `grazing_angle` → `beam_angle` refs | +| `test/test_store_import.cpp` | Update stale comment; (optional) add a Lambert-on offline case | +| `docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md` | New: transition addendum | ## Principles Self-Check | Principle | Consideration | |---|---| -| Enforcement over documentation | Add 5 targeted tests that would catch the wrong formula, wrong sign, and missing NaN/grazing guards — enforcement via test, not just comments | -| Capture decisions | ADR-0007 transition note records Phase B end date, formula chosen, what's deferred | -| A change includes its consequences | `test_store_import.cpp` fix included in same PR; both live (#78) and offline (#80) paths automatically corrected via the shared `extractNodeRecord()` | -| Only what's needed | Full GeoCoder correction (#15, #59) explicitly deferred; no per-path duplication | -| Improve incrementally | Phase B → first-cut Lambert → full GeoCoder (#15) — keeps each step reviewable | +| Enforcement over documentation | 6 targeted tests catch wrong formula, wrong sign, missing NaN/grazing guards, and the default-off path | +| Only what's needed | Flat-bottom Lambert only; full GeoCoder (#15/#59) deferred; one shared site, no per-path duplication | +| A change includes its consequences | Param plumbed through both estimator entry points; #78 + #80 corrected together; tests + stale comment updated in-PR | +| Capture decisions | ADR-0007 transition addendum records the formula, the operator-gated design, and the deferral | +| Robustness / sonar-agnostic | Correction is operator-set per sonar (the estimator cannot know the sonar's internal normalization); safe default off | ## ADR Compliance | ADR | Triggered | How addressed | |---|---|---| -| ADR-0007 D3 (incidence correction at node-output) | Yes — primary | Implemented here at `extractNodeRecord()` | -| ADR-0007 D2/D4 (per-beam mean + estimate variance) | Yes — already in place | No change; `corrected` replaces `raw_intensity` in the sum | -| ADR-0001 (adopt ADRs) | Yes | Transition note in `docs/decisions/` | -| ADR-0013 (progress.md vocabulary) | Yes | `## Plan Authored` entry committed with plan | +| ADR-0007 D3 (incidence correction at node-output) | Yes — primary | Implemented (param-gated) at `extractNodeRecord()` | +| ADR-0007 D2/D4 (per-beam mean + estimate variance) | Yes — in place | `corrected` replaces `raw` in the sum; unchanged combine | +| ADR-0008 (ROS 2 conventions) | Yes | New node param declared with default; offline CLI flag documented | +| ADR-0001 (adopt ADRs) | Yes | Transition addendum in `docs/decisions/` + follow-up to author the full ADR | ## Consequences -| If we change... | Also update... | Included in plan? | +| If we change... | Also update... | Included? | |---|---|---| -| `BeamIntensitySample::grazing_angle` name | `hypothesis.cpp`, `test_hypothesis.cpp`, `node.cpp` comment | Yes — Step 1 | -| `corrected` value in `extractNodeRecord()` | `test_node.cpp` non-nadir assertions | Yes — Step 3 | -| `corrected` value flows to `test_store_import.cpp` via `nodeRecords()` | `makeSoundingsWithIntensity` beam_angle=0.1 assertion | Yes — Step 4 | -| Phase B ends | ADR-0007 transition note | Yes — Step 5 | +| Add correction parameter | node ROS param + import_bag CLI + Parameters | Yes — Step 0 | +| `grazing_angle` field name | hypothesis.cpp, test_hypothesis.cpp, node.cpp comment, header docs | Yes — Step 1 | +| `corrected` value when param=Lambert | test_node.cpp (3 broken + 6 new) | Yes — Step 3 | +| Default None preserves current behavior | test_store_import.cpp default path unchanged; stale comment fixed | Yes — Step 4 | +| Phase B ends | ADR-0007 transition addendum + follow-up issue | Yes — Step 5 | | Full GeoCoder correction | cube#15, cube#59 | No — explicit follow-up | ## Open Questions -- [ ] Formula constant: this plan uses 10·log10(cos θ) (one-way Lambert, TVG - handled). If the M3 `reflectivity_db` already includes the Lambert receive - factor in its TVG (i.e., TVG corrects for cos θ), the correction double-counts. - Confirm from Kongsberg EM Datagram Formats that `reflectivity_db` is raw - backscatter without Lambert normalization before implementing. +- [x] **Lambert form** — RESOLVED (operator): cos²θ ⇒ `−20·log10(cos θ)`. +- [x] **Double-counting (is reflectivity_db pre-normalized?)** — RESOLVED + (operator): not knowable in the sonar-agnostic estimator → expose as an + operator startup parameter (default off). M3's correct setting is the operator's + to set per the sonar config; the bridge applies no angle compensation itself. ## Estimated Scope -Single PR. All changes touch one package (`cube_bathymetry`). ~120 lines changed, -~80 new test lines. +Single PR, one package. ~160 lines changed + ~110 new test lines (param plumbing +adds to the earlier estimate). From 321779012768f301fb5b0647676bfd1594af4193 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 11:21:51 +0000 Subject: [PATCH 07/17] progress: plan review for #81 --- .agent/work-plans/issue-81/progress.md | 30 ++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index 3f88dea..41de7c8 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -147,3 +147,33 @@ implementation and test. - [ ] (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` From d1eacd36fd2c797c5b862650dd990a614a52748d Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 12:14:18 +0000 Subject: [PATCH 08/17] =?UTF-8?q?plan(#81):=20replace=20cos=C2=B2=CE=B8=20?= =?UTF-8?q?Lambert=20with=20empirical=20ARA=20correction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Supersedes the prior plan: empirical analysis showed cos²θ explains ~¼ of the M3 nadir-to-edge falloff. New plan: offline derivation tool + configured curve + ARA interpolation at node-output, sonar-agnostic, default OFF. --- .agent/work-plans/issue-81/plan.md | 329 ++++++++++++++--------------- 1 file changed, 161 insertions(+), 168 deletions(-) diff --git a/.agent/work-plans/issue-81/plan.md b/.agent/work-plans/issue-81/plan.md index 59c6bb2..d6b084a 100644 --- a/.agent/work-plans/issue-81/plan.md +++ b/.agent/work-plans/issue-81/plan.md @@ -6,201 +6,194 @@ https://github.com/rolker/cube_bathymetry/issues/81 ## Context -`Node::extractNodeRecord()` (node.cpp:315-318) contains a Phase B no-op where -`corrected == raw` for every beam. The per-beam `{raw_intensity, beam_angle}` -pairs are already stored on each hypothesis (`Hypothesis::intensity_samples`). - -**Gate (rx_angles sign/zero convention) — VERIFIED host-side, PASS.** -`kongsberg_em_bridge/node.py:364-373` negates the Kongsberg raw pointing angle -(`+PORT → +STARBOARD`, matching `marine_acoustic_msgs`); nadir = 0 (boresight; -physical mount is in TF), radians. For a flat-bottom Lambert using `|angle|` the -sign is moot (cos is even); nadir=0 and `|angle|`=incidence-from-nadir both hold. - -**Is `reflectivity_db` already angle-normalized? — operator-controlled, NOT -hardcoded (operator decision).** `em_datagrams.py:120-126` reads `reflectivity_db` -straight from the EM **"Raw Range and Angle 78"** datagram (signed 16-bit ×0.1 dB) -with **no angle compensation in the bridge**. Whether the *sonar firmware* -pre-normalized that reflectivity for incidence is sonar/config-dependent and is -NOT determinable from workspace code. **This estimator is sonar-agnostic** (it -also runs Norbit, R2Sonic, sim, …), so whether to apply the Lambert correction -**must be an operator-set startup parameter**, not an M3 assumption baked into the -shared estimator. Default is **off (identity = current Phase B behavior)** — safe -and sonar-agnostic; a platform/config enables `lambert` for a sonar known to -deliver raw (non-angle-normalized) backscatter. - -The `BeamIntensitySample::grazing_angle` field (hypothesis.h:49) is **misnamed**: -it stores the receive steering / incidence angle from nadir (same semantics as -`sounding.beam_angle`), NOT a true grazing angle (90° − θ). The field doc already -says "receive/steering (beam/incidence) angle", so the comment is correct but the -name is wrong. Plan renames it to `beam_angle`. - -ADR-0007 has no document file in `docs/decisions/` yet; this PR adds a Phase B -transition **addendum** (not claiming the `0007-` slot) and files a follow-up -task for the full ADR-0007 doc. - -## Lambert Formula (operator-confirmed cos²θ) - -`raw_intensity` is `reflectivity_db` (dB). The angular correction is **additive in -dB** and applied ONLY when the operator parameter selects `lambert`: - -``` -corrected_dB = raw_dB − 20·log10(cos θ) θ = std::abs(sample.beam_angle) -``` - -**Form rationale (cos²θ / factor 20 — the classic seafloor Lambert law, -operator-confirmed; the reviewer flagged the earlier ×10 justification as -physically unsound):** -- Lambert backscatter cross-section per unit area `σ(θ) ∝ cos²θ` — a single - seabed-scattering term (insonified-footprint projection cosine × Lambertian - re-emission cosine), NOT a transmit/receive split. In dB: - `BS(θ) = BS(0) + 20·log10(cos θ)`. TVG handles range spreading/absorption, not - this angular factor — so it is not already removed. -- Normalizing to the nadir-equivalent ⇒ subtract the angular term: - `corrected_dB = raw_dB − 20·log10(cos θ)`. -- Nadir (θ=0): `cos0=1`, `log10(1)=0` → correction 0 (identity). ✓ -- Off-nadir (θ>0): `cos θ<1` → `−20·log10(cos θ) > 0` → boosts the weaker - off-nadir return toward its nadir-equivalent. - -**Near-grazing handling — single unambiguous rule (resolves the prior -contradiction):** beams with `θ ≥ MAX_INCIDENCE_RAD` (= 80° = 1.39626 rad) are -emitted **UNCORRECTED (identity)** — there is **no θ-clamping**. Rationale: the -Lambert model fails and `−20·log10(cos θ)` diverges near grazing; a clamp would -emit a physically meaningless frozen-at-80° value. Identity-beyond-max is the one -behavior; the test asserts identity beyond the cap. (The step at the cap is -accepted: those beams are unreliable either way.) -- NaN `beam_angle` → identity (consistent with `recordBeam` retaining NaN-angle - beams as "no angle correction available"). - -**This formula was the highest-risk decision; it has been operator-confirmed -(cos²θ) and independently flagged by review-plan. Re-review the revised plan -before implementing.** +**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. **Add the sonar-agnostic correction parameter** to `Parameters` - (`parameters.h`): an enum - `enum class BackscatterAngleCorrection { None, Lambert };` and a field - `BackscatterAngleCorrection backscatter_angle_correction = BackscatterAngleCorrection::None;` - (default None = identity = current behavior). Plumb it as a startup setting on - both estimator entry points: - - **Live node** (`cube_bathymetry_node.cpp`): declare a ROS parameter - `backscatter_angle_correction` (string `"none"`|`"lambert"`, default - `"none"`), parse into the enum when building `Parameters`. - - **Offline import** (`import_bag_main.cpp`): a `--backscatter-correction - none|lambert` flag (default `none`), documented in `usage()`. - Both run the same estimator, so one parameter corrects both the live (#78) and - offline (#80) paths consistently when enabled. - -1. **Rename `BeamIntensitySample::grazing_angle` → `beam_angle`** — struct + - field doc (`hypothesis.h:40-50`, **incl. the doc text at `hypothesis.h:115-116` - and the `BeamIntensitySample` reference at `hypothesis.h:166`**), `recordBeam` - signature/impl (`hypothesis.h:118`, `hypothesis.cpp:118,122,127`), the - `node.cpp` call-site comment, and test references - (`test_hypothesis.cpp:216,218,227,230,243`). - -2. **Replace the Phase B no-op in `node.cpp:315-318`** — when - `parameters.backscatter_angle_correction == Lambert`, set - `corrected = raw − 20·log10(cos(std::abs(beam_angle)))` with the near-grazing / - NaN identity rules above; otherwise `corrected = raw` (identity). Add the inline - comment capturing the producer cross-check (Kongsberg +PORT→negate→+STARBOARD; - nadir=0; radians; dB) AND that the correction is operator-gated because the - sonar-internal normalization state is not knowable here. Update the stale - `TODO(#54-B / #15)` block (`node.cpp:298-312`): Phase B done for the flat-bottom - case; retain the follow-up note for the full GeoCoder slope-aware correction - (#15/#59). - -3. **Update `test_node.cpp`** — enumerate **all three** non-nadir intensity - assertions that break under `lambert` (the new tests run with the param ON): - - `NodeRecordMeanAndEstimateVariance` — the **mean** assertion changes; the - **variance** assertion still holds (all four beams share one angle, so the - per-beam offset is constant and cancels in the variance). - - `FirstBeamInitializationRecordsIntensity` (`test_node.cpp:382`, - `EXPECT_FLOAT_EQ(record.intensity, -30.0f)`). - - `NodeRecordSkipsNanIntensityBeam` (`test_node.cpp:420`). - These existing tests stay on the **default (None)** path unless they opt into - `lambert`; assertions that exercise the correction must set the param ON. - Add Lambert tests (param = Lambert): - - `LambertCorrectionNonNadir`: θ=0.5 rad ⇒ `corrected = raw − 20·log10(cos 0.5)`, - `corrected ≠ raw`. - - `LambertCorrectionNadirIdentity`: θ≈0 ⇒ `corrected == raw` (float tol). - - `LambertCorrectionPortStarboardSymmetry`: ±0.3 rad ⇒ equal corrected value. - - `LambertCorrectionNaNAngle`: NaN angle ⇒ identity. - - `LambertCorrectionNearGrazing`: θ ≥ MAX_INCIDENCE_RAD ⇒ identity. - - `CorrectionOffIsIdentity`: param = None ⇒ `corrected == raw` for a non-nadir - beam (guards the default path). - -4. **`test_store_import.cpp` (#80 backscatter test) stays valid by default** — - the offline tests build `Parameters` with the default (None), so - `BackscatterCellsMatchGridRecords` (uses `beam_angle = 0.1f`, asserts surfaced - value == `kIntensity`) **still holds unchanged** (no correction by default). - **Update the stale comment** at `test_store_import.cpp:64-67` ("the surfaced - value is uncorrected so the angle does not change it here") to say *by default* - (None); optionally add a focused case that sets the param to Lambert and - asserts the corrected value flows through the offline path. (Preferred over - zeroing `beam_angle`, per review-plan — keeps a non-nadir beam exercising it.) - -5. **Add ADR-0007 transition addendum** — create +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 canonical `0007-…` slot before - the full ADR exists): records Phase B end, the operator-gated flat-bottom - Lambert (cos²θ), the sonar-agnostic parameter + default-off rationale, and what - stays deferred (full GeoCoder incidence via #15/#59). **File a separate GitHub - issue** to author the full ADR-0007 document. + (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/parameters.h` | Add `BackscatterAngleCorrection` enum + `backscatter_angle_correction` field (default None) | -| `include/cube_bathymetry/hypothesis.h` | Rename `grazing_angle` → `beam_angle` (struct + `recordBeam` decl + doc comments at 115-116, 166) | -| `src/hypothesis.cpp` | Rename param in `recordBeam` impl | -| `src/node.cpp` | Param-gated cos²θ Lambert correction; producer cross-check comment; update TODO block | -| `src/cube_bathymetry_node.cpp` | Declare/parse `backscatter_angle_correction` ROS param → Parameters | -| `src/import_bag_main.cpp` | `--backscatter-correction none|lambert` flag + usage() text → Parameters | -| `test/test_node.cpp` | Fix 3 broken non-nadir assertions; add 6 Lambert/param tests | -| `test/test_hypothesis.cpp` | `grazing_angle` → `beam_angle` refs | -| `test/test_store_import.cpp` | Update stale comment; (optional) add a Lambert-on offline case | -| `docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md` | New: transition addendum | +| `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 | |---|---| -| Enforcement over documentation | 6 targeted tests catch wrong formula, wrong sign, missing NaN/grazing guards, and the default-off path | -| Only what's needed | Flat-bottom Lambert only; full GeoCoder (#15/#59) deferred; one shared site, no per-path duplication | -| A change includes its consequences | Param plumbed through both estimator entry points; #78 + #80 corrected together; tests + stale comment updated in-PR | -| Capture decisions | ADR-0007 transition addendum records the formula, the operator-gated design, and the deferral | -| Robustness / sonar-agnostic | Correction is operator-set per sonar (the estimator cannot know the sonar's internal normalization); safe default off | +| 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 | Implemented (param-gated) at `extractNodeRecord()` | -| ADR-0007 D2/D4 (per-beam mean + estimate variance) | Yes — in place | `corrected` replaces `raw` in the sum; unchanged combine | -| ADR-0008 (ROS 2 conventions) | Yes | New node param declared with default; offline CLI flag documented | -| ADR-0001 (adopt ADRs) | Yes | Transition addendum in `docs/decisions/` + follow-up to author the full ADR | +| 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? | |---|---|---| -| Add correction parameter | node ROS param + import_bag CLI + Parameters | Yes — Step 0 | -| `grazing_angle` field name | hypothesis.cpp, test_hypothesis.cpp, node.cpp comment, header docs | Yes — Step 1 | -| `corrected` value when param=Lambert | test_node.cpp (3 broken + 6 new) | Yes — Step 3 | -| Default None preserves current behavior | test_store_import.cpp default path unchanged; stale comment fixed | Yes — Step 4 | -| Phase B ends | ADR-0007 transition addendum + follow-up issue | Yes — Step 5 | -| Full GeoCoder correction | cube#15, cube#59 | No — explicit follow-up | +| `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 -- [x] **Lambert form** — RESOLVED (operator): cos²θ ⇒ `−20·log10(cos θ)`. -- [x] **Double-counting (is reflectivity_db pre-normalized?)** — RESOLVED - (operator): not knowable in the sonar-agnostic estimator → expose as an - operator startup parameter (default off). M3's correct setting is the operator's - to set per the sonar config; the bridge applies no angle compensation itself. +- [ ] **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. ~160 lines changed + ~110 new test lines (param plumbing -adds to the earlier estimate). +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. From 0f4341488adeca658910a82e8e0d8efcdf9cf317 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 12:14:34 +0000 Subject: [PATCH 09/17] progress: plan authored for #81 --- .agent/work-plans/issue-81/progress.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index 41de7c8..b4cb0eb 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -177,3 +177,16 @@ file yet (only `0001-` exists). Formula has no objections. - [ ] (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. From e57093fbdb79e90bbc8453d02454bfa0f57401f5 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 12:24:23 +0000 Subject: [PATCH 10/17] progress: plan review for #81 --- .agent/work-plans/issue-81/progress.md | 32 ++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index b4cb0eb..da81c9e 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -190,3 +190,35 @@ file yet (only `0001-` exists). Formula has no objections. ### 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. From 6e22cbc50209e48a67272d30df26d425dfeb3052 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 08:51:04 -0400 Subject: [PATCH 11/17] refactor: rename BeamIntensitySample::grazing_angle to beam_angle The field stores the receive/steering (beam/incidence) angle from nadir, not a true grazing angle (90deg - theta). Rename it everywhere -- struct field, recordBeam param, docs, and tests -- to match what it holds. Part of #81 --- .../include/cube_bathymetry/hypothesis.h | 19 ++++++++++--------- cube_bathymetry/src/hypothesis.cpp | 6 +++--- cube_bathymetry/test/test_hypothesis.cpp | 8 ++++---- 3 files changed, 17 insertions(+), 16 deletions(-) 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/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/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 From f8a762cd9604beb46847df4380835e5e52d2af83 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 09:29:59 -0400 Subject: [PATCH 12/17] feat: add backscatter angular-response correction parameters + plumbing Add BackscatterAngleCorrection {None, Empirical} and an angular_response_curve to Parameters, a GeoMapSheet::setBackscatterCorrection() setter (grids hold a const ref to the sheet Parameters, so the setting reaches them), and a shared loadAngularResponseCurve()/parseBackscatterAngleCorrection() CSV loader. Plumb both estimator entry points: the live node declares backscatter_angle_correction + backscatter_curve_file ROS params; the offline importer adds --backscatter-correction + --backscatter-curve. Empirical mode with an empty/missing curve logs a loud no-op warning rather than failing silently. Default none preserves the Phase B identity behavior. Part of #81 --- cube_bathymetry/CMakeLists.txt | 1 + .../cube_bathymetry/angular_response_curve.h | 53 ++++++++ .../include/cube_bathymetry/geo_map_sheet.h | 12 ++ .../include/cube_bathymetry/parameters.h | 28 +++++ .../src/angular_response_curve.cpp | 117 ++++++++++++++++++ cube_bathymetry/src/cube_bathymetry_node.cpp | 40 ++++++ cube_bathymetry/src/geo_map_sheet.cpp | 9 ++ cube_bathymetry/src/import_bag_main.cpp | 52 +++++++- 8 files changed, 310 insertions(+), 2 deletions(-) create mode 100644 cube_bathymetry/include/cube_bathymetry/angular_response_curve.h create mode 100644 cube_bathymetry/src/angular_response_curve.cpp diff --git a/cube_bathymetry/CMakeLists.txt b/cube_bathymetry/CMakeLists.txt index 4c4c41d..c4b7de5 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 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/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/src/angular_response_curve.cpp b/cube_bathymetry/src/angular_response_curve.cpp new file mode 100644 index 0000000..21a4eb3 --- /dev/null +++ b/cube_bathymetry/src/angular_response_curve.cpp @@ -0,0 +1,117 @@ +// 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); + // Reject rows where the numeric parse did not consume the field (e.g. the + // header's "abs_angle_deg_center" leading char would throw, but a value + // like "1deg" would parse a prefix -- guard against that too). + 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/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; From 86c72e9e29df62b82a4e7198eda5f1fafef01371 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 09:32:21 -0400 Subject: [PATCH 13/17] feat: empirical ARA backscatter correction + offline derivation tool Replace the Phase B node-output no-op (corrected == raw) with the empirical angular-response correction: corrected_dB = raw_dB - curveRel(|beam_angle|), where curveRel is the per-sonar curve's db_relative_to_nadir column, linearly interpolated between bin centres by |beam_angle| in degrees. Nadir, beyond-max angle, NaN angle, mode None, or an empty curve all fall back to identity. The curve is keyed on |angle| so port/starboard beams of equal magnitude match. The rx_angles sign/zero gate (kongsberg_em_bridge negates +PORT to +STARBOARD, nadir 0, radians, dB) is documented at the correction site. Add scripts/derive_angular_response.py: reads a SonarDetections bag, bins intensities by |rx_angles| (2-deg bins), writes the curve CSV the correction consumes. Installed to lib/cube_bathymetry. Part of #81 --- cube_bathymetry/CMakeLists.txt | 8 + .../scripts/derive_angular_response.py | 154 ++++++++++++++++++ cube_bathymetry/src/node.cpp | 91 +++++++++-- 3 files changed, 237 insertions(+), 16 deletions(-) create mode 100755 cube_bathymetry/scripts/derive_angular_response.py diff --git a/cube_bathymetry/CMakeLists.txt b/cube_bathymetry/CMakeLists.txt index c4b7de5..274fa62 100644 --- a/cube_bathymetry/CMakeLists.txt +++ b/cube_bathymetry/CMakeLists.txt @@ -143,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 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/node.cpp b/cube_bathymetry/src/node.cpp index 7db12a1..ee95666 100644 --- a/cube_bathymetry/src/node.cpp +++ b/cube_bathymetry/src/node.cpp @@ -23,10 +23,55 @@ #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: identity (no extrapolation past measured extent). + if (abs_angle_deg > curve.back().first) { + return 0.0; + } + // 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 +340,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; From 559bd4775f0d09a92b0a963a346ac8f717d148ac Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 09:32:30 -0400 Subject: [PATCH 14/17] test: ARA correction + curve-loader tests; backscatter comment hygiene Add 6 ARA cases at extractNodeRecord (interpolation at a mid angle, nadir identity, beyond-max identity, NaN-angle identity, mode-None identity, and port/starboard symmetry) plus curve-loader/mode-parse tests (test_angular_response_curve). The 3 pre-existing default-path intensity assertions are unchanged: default None means corrected == raw. Update stale 'uncorrected' comments in test_store_import to note the surface is uncorrected by default (BackscatterAngleCorrection::None). Part of #81 --- cube_bathymetry/CMakeLists.txt | 3 + .../test/test_angular_response_curve.cpp | 114 ++++++++++++++++++ cube_bathymetry/test/test_node.cpp | 111 +++++++++++++++++ cube_bathymetry/test/test_store_import.cpp | 12 +- 4 files changed, 235 insertions(+), 5 deletions(-) create mode 100644 cube_bathymetry/test/test_angular_response_curve.cpp diff --git a/cube_bathymetry/CMakeLists.txt b/cube_bathymetry/CMakeLists.txt index 274fa62..2dc4667 100644 --- a/cube_bathymetry/CMakeLists.txt +++ b/cube_bathymetry/CMakeLists.txt @@ -221,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/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_node.cpp b/cube_bathymetry/test/test_node.cpp index 4f60deb..12ea85a 100644 --- a/cube_bathymetry/test/test_node.cpp +++ b/cube_bathymetry/test/test_node.cpp @@ -570,4 +570,115 @@ 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, ARABeyondMaxAngleIdentity) +{ + 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, -30.0f, 1e-4); +} + +// 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); From 271e52fa3519968b8c66d3992a5885cd847c8bc5 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 09:34:15 -0400 Subject: [PATCH 15/17] docs: ADR-0007 addendum for Phase B -> empirical ARA transition Record the end of the Phase B identity no-op and the empirical angular-response correction: the configured-curve + default-off rationale, the offline derivation tool, the verified rx_angles gate, and what is deferred (full radiometric GeoCoder -- area/beam-pattern/TVG/slope, ADR-0007 D3 / #15 / #59). Notes follow-ups to author the canonical ADR-0007 document and to model intensity domain (dB vs linear) as a sonar property. Addendum name only -- does not claim the canonical 0007 slot. Part of #81 --- ...atter-store-addendum-phase-b-transition.md | 103 ++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 cube_bathymetry/docs/decisions/0007-mbes-backscatter-store-addendum-phase-b-transition.md 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. From 099aed37f5c0bc7ccebf0abb9702f38f34615d03 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 13:53:44 +0000 Subject: [PATCH 16/17] progress: local review for #81 --- .agent/work-plans/issue-81/progress.md | 30 ++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/.agent/work-plans/issue-81/progress.md b/.agent/work-plans/issue-81/progress.md index da81c9e..ccd4420 100644 --- a/.agent/work-plans/issue-81/progress.md +++ b/.agent/work-plans/issue-81/progress.md @@ -222,3 +222,33 @@ 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`). From 7e538caab34375379c8fed2dba877914542533c8 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 28 Jun 2026 10:15:54 -0400 Subject: [PATCH 17/17] fix(ara): clamp beyond-max correction to edge; loader comment accuracy (#81 review) Pre-push review-code suggestions: - curveRelativeDb: beyond the curve's last bin, clamp to the outermost bin's correction instead of returning identity (0). The empirical curve is bounded (unlike a diverging cos/log model), so continuing the edge value keeps the correction continuous and avoids a ~24 dB swath-edge discontinuity / bright ring. Test renamed ARABeyondMaxAngleClampsToEdge, asserts the clamped value. - angular_response_curve loader: correct the comment overstating the used==0 guard (std::stof accepts a numeric prefix like "1deg"=1.0; not rejected). 378 tests green. Part of #81 --- cube_bathymetry/src/angular_response_curve.cpp | 8 +++++--- cube_bathymetry/src/node.cpp | 7 +++++-- cube_bathymetry/test/test_node.cpp | 7 +++++-- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/cube_bathymetry/src/angular_response_curve.cpp b/cube_bathymetry/src/angular_response_curve.cpp index 21a4eb3..0a12f5e 100644 --- a/cube_bathymetry/src/angular_response_curve.cpp +++ b/cube_bathymetry/src/angular_response_curve.cpp @@ -90,9 +90,11 @@ std::vector> loadAngularResponseCurve(const std::string 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); - // Reject rows where the numeric parse did not consume the field (e.g. the - // header's "abs_angle_deg_center" leading char would throw, but a value - // like "1deg" would parse a prefix -- guard against that too). + // 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; } diff --git a/cube_bathymetry/src/node.cpp b/cube_bathymetry/src/node.cpp index ee95666..420bd71 100644 --- a/cube_bathymetry/src/node.cpp +++ b/cube_bathymetry/src/node.cpp @@ -48,9 +48,12 @@ double curveRelativeDb( if (abs_angle_deg <= curve.front().first) { return curve.front().second; } - // Beyond the last bin centre: identity (no extrapolation past measured extent). + // 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 0.0; + return curve.back().second; } // Find the bracketing pair [lo, hi] and linearly interpolate. for (std::size_t i = 1; i < curve.size(); ++i) { diff --git a/cube_bathymetry/test/test_node.cpp b/cube_bathymetry/test/test_node.cpp index 12ea85a..56fea50 100644 --- a/cube_bathymetry/test/test_node.cpp +++ b/cube_bathymetry/test/test_node.cpp @@ -626,15 +626,18 @@ TEST_F(NodeTest, ARANadirIdentity) } // Beyond the curve's max angle (60 deg): identity (no extrapolation). -TEST_F(NodeTest, ARABeyondMaxAngleIdentity) +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, -30.0f, 1e-4); + EXPECT_NEAR(record.intensity, -18.0f, 1e-3); } // NaN beam angle: no correction applicable -> identity.