diff --git a/docs/tuning/2026-08-11-orientation-picking.md b/docs/tuning/2026-08-11-orientation-picking.md new file mode 100644 index 0000000..e2b25ad --- /dev/null +++ b/docs/tuning/2026-08-11-orientation-picking.md @@ -0,0 +1,136 @@ +# Orientation picking — the phantom wall direction, 2026-08-11 + +**Verdict: the declutter pass picks wall orientations by their height above the +angular energy's broadband floor (`peak_rel_threshold` 0.15 of the residual +maximum), not by their height above zero (0.45 of the raw maximum).** The +phantom direction that forced a hand-tuned threshold on the 2026-08-02 flat map +is gone, and the shipped defaults now reproduce that hand-tuned output exactly. +This doc is the measurement behind the change. Data: +`2026-08-11-orientation-picking/`. + +## The defect + +`_pick_directions` kept any local maximum of the angular energy reaching +`peak_rel_threshold` (0.45) of the global maximum, after suppressing peaks +within 12° of a stronger one. + +Clutter, speckle and ragged wall edges put energy at *every* orientation, so a +real map's angular energy is a modest set of wall peaks riding on a broad +pedestal — **measured at 0.49 of the global maximum** on the 2026-08-02 flat +map. A threshold expressed as a fraction of the raw maximum therefore spends +more than half its range below the pedestal, where nothing is a direction and +everything clears the gate. + +What survives that gate is whatever the 12° suppression radius happens to +leave: on that map, a direction at **73.8°**, which is the shoulder of the real +86.8° wall family 13.0° away — just outside the radius. It reaches 0.50 of the +raw maximum against the weakest real family's 0.64, a ratio of 1.3, and no +threshold placed between two such numbers is a measurement. The operator's +workaround was `--peak-rel 0.55`, threaded between them by hand. + +It is not one map's curiosity. Of the **13 genuine occupancy maps** on this +box, **6 carried such a shoulder**, every one of them 12.0–13.5° from a +stronger peak. + +![before and after](2026-08-11-orientation-picking/energy-panel-before-after.png) + +## What ROSE itself does, and why this departs from it + +Worth stating plainly, because this module cites ROSE: **the shipped +relative-height rule was never ROSE's rule**, and neither is the one replacing +it. ROSE (Luperto et al., [arXiv:2004.08794](https://arxiv.org/abs/2004.08794), +§3.1) selects directions by **topographic prominence**, thresholded at 50% of +the curve's peak-to-trough range — literally, in the reference implementation +([aislabunimi/ROSE2](https://github.com/aislabunimi/ROSE2), +`src/rose_v1_repo/fft_structure_extraction.py`): + +```python +find_peaks(pol_h, prominence=(np.max(pol_h) - np.min(pol_h)) * 0.5) +``` + +Run over our corpus (`rose-rule.txt`), that criterion is *more* conservative +than either of ours: on all 13 maps it returns exactly the two strongest, +near-orthogonal directions, and never a phantom. It also drops the real +off-axis families along with them — 11.8° and 110.2° on the flat map — which is +the two-direction result the operator rejected at `--peak-rel 0.65` for visibly +eroding walls. + +That is a difference in the maps, not a mistake in the paper. ROSE scores large, +overwhelmingly rectilinear building floor plans, where two directions is the +right answer and a third is usually clutter; and its output is an abstracted +floor plan. A small flat mapped by a 2-D lidar has genuine off-axis wall +families, and this pass keeps *observed pixels* gated to real walls, so a +dropped family costs real wall rather than detail. + +## Two fixes that do not work + +**Literal topographic prominence**, at a threshold loose enough to keep the four +families, cannot exclude the phantom. The real off-axis family at 11.8° scores +0.033 of the maximum against the phantom's 0.028 — that family sits on the tail +of the dominant one and is a shoulder too, in exactly the same sense. So +prominence has no setting that expresses what is wanted here: the paper's +threshold gives two directions, and any threshold that admits the third and +fourth admits the phantom with them. Thresholding it that low is worse still, +because prominence rewards isolation and starts promoting lone bumps in the +noise floor that carry no structural energy at all (47.2° on the tuning map; +43.2° and 137.2° on the replay maps). + +**A wider suppression radius** cannot separate them either. The phantom is +13.0° from its parent; the real off-axis family is 14.5° from its own. Any +radius that suppresses the one suppresses the other. + +## The fix + +Subtract the broadband floor before thresholding — the curve smoothed over +`FLOOR_HALFWIDTH_DEG` (45°), i.e. what is left once anything with a wall's +angular sharpness is averaged away. This module already computed that residual +for `angular_stats`; the declutter picker was simply not using it. + +On the same two peaks, the residual reads **0.012 for the phantom and 0.296 for +the weakest real family** — 1.3× apart before, 24× apart after. + +The threshold moves with the curve: 0.15 of the residual maximum, the same +number `angular_stats` already used on the same kind of curve. **Both halves +have to travel together.** 0.45 on the residual would drop real families; 0.15 +on the raw curve returns four directions on a clean synthetic rectilinear map, +two of them flank samples. A test pins that combination. + +The threshold's window is measured, and is not wide: + +| bound | map | direction | residual | +| --- | --- | --- | --- | +| from below | `sim/mote_world` | 137.2° phantom, admitted at 0.10 | 0.109 | +| from above | `tuning/input_map` | 16.2°, a real family the shipped rule kept | 0.189 | + +## What changed, over every map on the box + +`corpus.txt` — 13 genuine ROS occupancy PNGs (the tuning input, the three sim +world maps, and eight `map_raw_notraj.png` outputs of the bag-replay harness; +`bag_replay_results/*/*/map.png` is a rendered *figure*, not a map, and is not +in the corpus). + +* **7 maps unchanged**, including the whole tuning set and all three sim worlds. +* **6 maps lost exactly one direction each**, and every one of them was a + shoulder 12.0–13.0° from a stronger peak, with residual 0.012–0.105 against a + weakest-kept of 0.204–0.285. +* **No map gained a direction.** + +## Does it reproduce the hand-tuned map? + +Yes, byte for byte. The 2026-08-02 flat map cleaned with the new defaults is +**identical** to the `--peak-rel 0.55` output the operator hand-threaded and +shipped (0 differing cells of 43 259), and differs from the old default's output +by 664 cells. `flat-map-output.txt`. + +Room segmentation, which picks directions through the same function to find the +map's dominant rotation, is unchanged on the sim ladder — 30/33 hospital, 10/10 +office, 1/1 mote, zero merges, and the same again with the map turned +17° and +−31°. + +## What this does not fix + +The picker still returns *directions*, not families, and `max_directions` (5) is +still a cap rather than a decision: a building genuinely using six wall +directions loses one, silently. Nothing here estimates how many directions a map +*should* have — `angular_stats`'s frame table is the diagnostic that reads the +answer back, and it is a diagnostic, not a gate. diff --git a/docs/tuning/2026-08-11-orientation-picking/corpus.txt b/docs/tuning/2026-08-11-orientation-picking/corpus.txt new file mode 100644 index 0000000..815fd11 --- /dev/null +++ b/docs/tuning/2026-08-11-orientation-picking/corpus.txt @@ -0,0 +1,33 @@ +# Direction sets over every genuine ROS occupancy PNG on the dev box, 2026-08-11. +# "before" reproduces the shipped rule (relative height on the raw angular +# energy, 0.45); "after" is the patched pipeline (relative height on the +# floor-subtracted residual, 0.15). Angles are spectral (wall normals), as the +# declutter CLI prints them. +# +# Corpus: scratchpad_results/map_cleanup/{input_map,warehouse/cleaned}.png, +# mote_simulation/sim_home/sites/*/floors/ground/maps/*/map.png, and +# bag_replay_results/*/*/map_raw_notraj.png. + +map before after directions +-------------------------------------------------------------------------------------------------------------- +tuning/input_map 4 4 UNCHANGED +tuning/warehouse 1 1 UNCHANGED +sim/hospital_world 2 2 UNCHANGED +sim/mote_world 2 2 UNCHANGED +sim/office_world 2 2 UNCHANGED +replay/20260802T142558/r2-chain10 5 4 dropped 73.75 + dropped 73.75: raw h/max 0.501 residual 0.012 raw prominence/max 0.028 gap to stronger peak 13.0 deg (weakest kept residual 0.285) +replay/20260802T162552/r2-chain10 5 5 UNCHANGED +replay/20260802T162659/r2-chain10 5 4 dropped 170.25 + dropped 170.25: raw h/max 0.472 residual 0.070 raw prominence/max 0.014 gap to stronger peak 12.0 deg (weakest kept residual 0.204) +replay/20260802T162758/r2-chain10 5 4 dropped 96.75 + dropped 96.75: raw h/max 0.538 residual 0.105 raw prominence/max 0.000 gap to stronger peak 12.0 deg (weakest kept residual 0.235) +replay/20260802T163028/r2-chain10 5 4 dropped 100.75 + dropped 100.75: raw h/max 0.526 residual 0.028 raw prominence/max 0.000 gap to stronger peak 12.0 deg (weakest kept residual 0.270) +replay/20260802T163104/r2-chain10 5 4 dropped 98.25 + dropped 98.25: raw h/max 0.525 residual 0.104 raw prominence/max 0.000 gap to stronger peak 12.0 deg (weakest kept residual 0.259) +replay/20260802T163141/r2-chain10 4 4 UNCHANGED +replay/20260802T163259/r2-chain10-finang 5 4 dropped 80.25 + dropped 80.25: raw h/max 0.533 residual 0.065 raw prominence/max 0.000 gap to stronger peak 12.0 deg (weakest kept residual 0.279) +-------------------------------------------------------------------------------------------------------------- +7 maps unchanged, 6 changed diff --git a/docs/tuning/2026-08-11-orientation-picking/energy-panel-before-after.png b/docs/tuning/2026-08-11-orientation-picking/energy-panel-before-after.png new file mode 100644 index 0000000..7f2394a Binary files /dev/null and b/docs/tuning/2026-08-11-orientation-picking/energy-panel-before-after.png differ diff --git a/docs/tuning/2026-08-11-orientation-picking/flat-map-output.txt b/docs/tuning/2026-08-11-orientation-picking/flat-map-output.txt new file mode 100644 index 0000000..2e84f7a --- /dev/null +++ b/docs/tuning/2026-08-11-orientation-picking/flat-map-output.txt @@ -0,0 +1,10 @@ +# The 2026-08-02 flat map through the new defaults, against the outputs cleaned +# on 2026-08-02. `cleaned` is the old default (--peak-rel 0.45); `cleaned-rel*` +# are the hand-threaded runs. 0.55 is the one the operator shipped. + +new default directions: [11.75, 86.75, 110.25, 177.25] +occupied: 4474 -> 3245 + vs cleaned differing cells: 664 (1.535% of the grid) + vs cleaned-rel0.5 differing cells: 664 (1.535% of the grid) + vs cleaned-rel0.55 differing cells: 0 (0.000% of the grid) IDENTICAL + vs cleaned-rel0.65 differing cells: 1321 (3.054% of the grid) diff --git a/docs/tuning/2026-08-11-orientation-picking/rose-rule.txt b/docs/tuning/2026-08-11-orientation-picking/rose-rule.txt new file mode 100644 index 0000000..3f59ace --- /dev/null +++ b/docs/tuning/2026-08-11-orientation-picking/rose-rule.txt @@ -0,0 +1,28 @@ +# ROSE's own peak criterion over our corpus, 2026-08-11, against this PR's. +# +# ROSE (arXiv:2004.08794 §3.1) selects dominant directions by topographic +# prominence at 50% of the curve's peak-to-trough range; the reference +# implementation (aislabunimi/ROSE2, src/rose_v1_repo/fft_structure_extraction.py) +# is find_peaks(pol_h, prominence=(max(pol_h) - min(pol_h)) * 0.5). Applied here +# to the same angular energy curve our declutter pass measures, with scipy's +# prominence definition transcribed (no scipy in the robot env). +# +# It never admits a phantom, and it never returns more than the two strongest +# near-orthogonal directions -- including on the maps whose real off-axis +# families this pass has to keep. Angles are spectral (wall normals). + +map ROSE (prom >= 0.5*range) this PR +---------------------------------------------------------------------------------------------------- +tuning/input_map 88.2 179.8 16.2 88.2 114.8 179.8 +tuning/warehouse 179.8 179.8 +sim/hospital_world 0.2 90.2 0.2 90.2 +sim/mote_world 0.2 90.2 0.2 90.2 +sim/office_world 90.2 179.2 90.2 179.2 +replay/20260802T142558/r2-chain10 86.8 177.2 11.8 86.8 110.2 177.2 +replay/20260802T162552/r2-chain10 80.2 174.8 7.2 80.2 92.2 104.8 174.8 +replay/20260802T162659/r2-chain10 2.2 90.8 2.2 14.8 90.8 114.8 +replay/20260802T162758/r2-chain10 84.8 178.2 10.8 84.8 109.2 178.2 +replay/20260802T163028/r2-chain10 88.2 179.8 13.2 88.2 112.8 179.8 +replay/20260802T163104/r2-chain10 86.2 178.2 11.2 86.2 110.8 178.2 +replay/20260802T163141/r2-chain10 2.2 92.8 2.2 14.8 92.8 114.8 +replay/20260802T163259/r2-chain10-finang 0.2 92.2 0.2 14.2 92.2 113.8 diff --git a/docs/tuning/2026-08-11-orientation-picking/threshold-sweep.txt b/docs/tuning/2026-08-11-orientation-picking/threshold-sweep.txt new file mode 100644 index 0000000..c2b1e27 --- /dev/null +++ b/docs/tuning/2026-08-11-orientation-picking/threshold-sweep.txt @@ -0,0 +1,139 @@ +# Threshold sweep behind peak_rel_threshold = 0.15, 2026-08-11. Same corpus as +# corpus.txt. Schemes, all with the shipped 12 deg suppression radius and a cap +# of 5 directions: +# raw @t : threshold t * max on the raw angular energy (what shipped) +# res @t : threshold t * max on the floor-subtracted residual (what ships now) +# prom@t : threshold t * max on topographic prominence of the raw curve +# prom@ is the rejected candidate: note the directions it invents (47.2 on the +# tuning map, 43.2 / 137.2 on the replay maps) and the real families it drops. + + +== tuning/input_map + raw @0.45 (ships): 16.25 88.25 114.75 179.75 + res @0.10 : 16.25 88.25 114.75 179.75 + res @0.15 : 16.25 88.25 114.75 179.75 + res @0.20 : 88.25 114.75 179.75 + res @0.25 : 88.25 114.75 179.75 + prom@0.03 : 16.25 47.25 88.25 114.75 179.75 + prom@0.05 : 16.25 47.25 88.25 114.75 179.75 + prom@0.08 : 16.25 47.25 88.25 114.75 179.75 + +== tuning/warehouse + raw @0.45 (ships): 179.75 + res @0.10 : 179.75 + res @0.15 : 179.75 + res @0.20 : 179.75 + res @0.25 : 179.75 + prom@0.03 : 179.75 + prom@0.05 : 179.75 + prom@0.08 : 179.75 + +== sim/hospital_world + raw @0.45 (ships): 0.25 90.25 + res @0.10 : 0.25 90.25 + res @0.15 : 0.25 90.25 + res @0.20 : 0.25 90.25 + res @0.25 : 0.25 90.25 + prom@0.03 : 0.25 90.25 + prom@0.05 : 0.25 90.25 + prom@0.08 : 0.25 90.25 + +== sim/mote_world + raw @0.45 (ships): 0.25 90.25 + res @0.10 : 0.25 90.25 137.25 + res @0.15 : 0.25 90.25 + res @0.20 : 0.25 90.25 + res @0.25 : 0.25 90.25 + prom@0.03 : 0.25 47.25 69.75 90.25 137.25 + prom@0.05 : 0.25 47.25 69.75 90.25 137.25 + prom@0.08 : 0.25 47.25 90.25 137.25 + +== sim/office_world + raw @0.45 (ships): 90.25 179.25 + res @0.10 : 90.25 179.25 + res @0.15 : 90.25 179.25 + res @0.20 : 90.25 179.25 + res @0.25 : 90.25 179.25 + prom@0.03 : 90.25 179.25 + prom@0.05 : 90.25 179.25 + prom@0.08 : 90.25 179.25 + +== replay/20260802T142558/r2-chain10 + raw @0.45 (ships): 11.75 73.75 86.75 110.25 177.25 + res @0.10 : 11.75 86.75 110.25 177.25 + res @0.15 : 11.75 86.75 110.25 177.25 + res @0.20 : 11.75 86.75 110.25 177.25 + res @0.25 : 11.75 86.75 110.25 177.25 + prom@0.03 : 47.25 86.75 110.25 137.25 177.25 + prom@0.05 : 47.25 86.75 110.25 137.25 177.25 + prom@0.08 : 47.25 86.75 110.25 137.25 177.25 + +== replay/20260802T162552/r2-chain10 + raw @0.45 (ships): 7.25 80.25 92.25 104.75 174.75 + res @0.10 : 7.25 80.25 92.25 104.75 174.75 + res @0.15 : 7.25 80.25 92.25 104.75 174.75 + res @0.20 : 7.25 80.25 92.25 104.75 174.75 + res @0.25 : 7.25 80.25 92.25 104.75 174.75 + prom@0.03 : 7.25 80.25 92.25 104.75 174.75 + prom@0.05 : 7.25 80.25 92.25 104.75 174.75 + prom@0.08 : 80.25 92.25 104.75 174.75 + +== replay/20260802T162659/r2-chain10 + raw @0.45 (ships): 2.25 14.75 90.75 114.75 170.25 + res @0.10 : 2.25 14.75 90.75 114.75 + res @0.15 : 2.25 14.75 90.75 114.75 + res @0.20 : 2.25 14.75 90.75 114.75 + res @0.25 : 2.25 90.75 114.75 + prom@0.03 : 2.25 14.75 73.75 90.75 114.75 + prom@0.05 : 2.25 14.75 73.75 90.75 114.75 + prom@0.08 : 2.25 14.75 90.75 114.75 + +== replay/20260802T162758/r2-chain10 + raw @0.45 (ships): 10.75 84.75 96.75 109.25 178.25 + res @0.10 : 10.75 84.75 96.75 109.25 178.25 + res @0.15 : 10.75 84.75 109.25 178.25 + res @0.20 : 10.75 84.75 109.25 178.25 + res @0.25 : 84.75 109.25 178.25 + prom@0.03 : 40.75 65.25 84.75 109.25 178.25 + prom@0.05 : 40.75 65.25 84.75 109.25 178.25 + prom@0.08 : 84.75 109.25 178.25 + +== replay/20260802T163028/r2-chain10 + raw @0.45 (ships): 13.25 88.25 100.75 112.75 179.75 + res @0.10 : 13.25 88.25 112.75 179.75 + res @0.15 : 13.25 88.25 112.75 179.75 + res @0.20 : 13.25 88.25 112.75 179.75 + res @0.25 : 13.25 88.25 112.75 179.75 + prom@0.03 : 13.25 88.25 112.75 151.25 179.75 + prom@0.05 : 13.25 88.25 112.75 151.25 179.75 + prom@0.08 : 13.25 88.25 112.75 179.75 + +== replay/20260802T163104/r2-chain10 + raw @0.45 (ships): 11.25 86.25 98.25 110.75 178.25 + res @0.10 : 11.25 86.25 98.25 110.75 178.25 + res @0.15 : 11.25 86.25 110.75 178.25 + res @0.20 : 11.25 86.25 110.75 178.25 + res @0.25 : 11.25 86.25 110.75 178.25 + prom@0.03 : 48.75 86.25 110.75 139.25 178.25 + prom@0.05 : 48.75 86.25 110.75 139.25 178.25 + prom@0.08 : 86.25 110.75 178.25 + +== replay/20260802T163141/r2-chain10 + raw @0.45 (ships): 2.25 14.75 92.75 114.75 + res @0.10 : 2.25 14.75 92.75 114.75 + res @0.15 : 2.25 14.75 92.75 114.75 + res @0.20 : 2.25 92.75 114.75 + res @0.25 : 2.25 92.75 + prom@0.03 : 2.25 14.75 73.75 92.75 114.75 + prom@0.05 : 2.25 14.75 73.75 92.75 114.75 + prom@0.08 : 2.25 92.75 114.75 + +== replay/20260802T163259/r2-chain10-finang + raw @0.45 (ships): 0.25 14.25 80.25 92.25 113.75 + res @0.10 : 0.25 14.25 92.25 113.75 + res @0.15 : 0.25 14.25 92.25 113.75 + res @0.20 : 0.25 14.25 92.25 113.75 + res @0.25 : 0.25 14.25 92.25 113.75 + prom@0.03 : 0.25 14.25 92.25 113.75 143.25 + prom@0.05 : 0.25 14.25 92.25 113.75 143.25 + prom@0.08 : 0.25 14.25 92.25 113.75 diff --git a/mkdocs.yml b/mkdocs.yml index d2153bd..d8b94ef 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -156,3 +156,4 @@ nav: - Slip detection: tuning/2026-07-28-slip-detection.md - Camera layer decay: tuning/2026-07-29-camera-layer-decay.md - Monitor-node CPU: tuning/2026-08-11-monitor-cpu.md + - Orientation picking: tuning/2026-08-11-orientation-picking.md diff --git a/mote_bringup/mote_bringup/map_cleanup/README.md b/mote_bringup/mote_bringup/map_cleanup/README.md index 11bfb6c..0d65dfe 100644 --- a/mote_bringup/mote_bringup/map_cleanup/README.md +++ b/mote_bringup/mote_bringup/map_cleanup/README.md @@ -18,7 +18,9 @@ orientations; clutter and speckle smear energy across *all* orientations. So: 1. binarise the map into a wall image, 2. take its 2D FFT and measure spectral energy as a function of angle, -3. pick the dominant orientations (peaks of that angular energy), +3. pick the dominant orientations — peaks of that angular energy measured + *above its broadband floor*, since the same clutter that smears energy + everywhere lifts every orientation at once, 4. keep only the frequency wedges aligned with those orientations — a directional band-pass — and invert the FFT to get a continuous structure score, @@ -30,10 +32,19 @@ The building does **not** have to be Manhattan (axis-aligned): the dominant orientations are whatever the map actually contains, including diagonal corridors. +Step 3 is where this **departs from ROSE deliberately**. ROSE picks directions +by topographic prominence at 50% of the angular curve's peak-to-trough range, +which on every map measured here returns exactly the two strongest, +near-orthogonal directions. That is right for the large, overwhelmingly +rectilinear floor plans ROSE scores, and wrong for a small flat mapped by a 2-D +lidar, which has genuine off-axis wall families that a two-direction filter +erodes. See `angular_stats._pick_directions` and +`docs/tuning/2026-08-11-orientation-picking.md`. + ## Usage ```bash -pixi run clean-map path/to/map.png [--out DIR] [--wedge 5] [--peak-rel 0.45] [--gate 2] +pixi run clean-map path/to/map.png [--out DIR] [--wedge 5] [--peak-rel 0.15] [--gate 2] ``` Writes `_cleaned.png` (a ROS occupancy PNG) and `_diagnostics.png` @@ -126,6 +137,17 @@ off-axis walls) and `peak_rel_threshold` (higher = fewer orientations kept). Direction detection sharpens considerably on full-resolution maps — the current validation input is a low-res screenshot, which inflates spectral side-lobes. +`peak_rel_threshold` (0.15) is a fraction of the strongest peak **measured above +the angular energy's broadband floor**, not of the raw curve's maximum, and the +two are not interchangeable: on a real map the floor is around half the maximum, +so a fraction of the raw height spends most of its range on clutter and admits +the *shoulder* of a real wall family as a direction of its own. Both halves of +that change have to travel together — the old 0.45 on the residual would drop +real families, and 0.15 on the raw curve accepts nearly anything. Why literal +topographic prominence is not the answer either is in +`angular_stats._pick_directions`; the measurements are in +`docs/tuning/2026-08-11-orientation-picking.md`. + `RoomParams` in `room_segmentation.py` governs the segmentation. `door_max_m` (1.4) is the width that still counts as a doorway rather than an opening, and `min_wall_run_m` (1.5) the shortest unbroken wall allowed to define a room — diff --git a/mote_bringup/mote_bringup/map_cleanup/angular_stats.py b/mote_bringup/mote_bringup/map_cleanup/angular_stats.py index 3b67795..2869c5d 100644 --- a/mote_bringup/mote_bringup/map_cleanup/angular_stats.py +++ b/mote_bringup/mote_bringup/map_cleanup/angular_stats.py @@ -77,6 +77,8 @@ "refine_peak", "_angular_energy", "_smooth_circular", + "_floor_subtract", + "_pick_peaks", "_pick_directions", "_angdist", ] @@ -96,7 +98,7 @@ class SpectrumParams: angle_step_deg: float = 0.5 lowcut_frac: float = 0.02 - peak_rel_threshold: float = 0.45 + peak_rel_threshold: float = 0.15 peak_nms_deg: float = 12.0 max_directions: int = 5 wedge_halfwidth_deg: float = 5.0 @@ -109,12 +111,14 @@ class SpectrumParams: FLOOR_HALFWIDTH_DEG = 45.0 # Relative peak threshold used when picking the direction families the stats are -# reported against. Deliberately *not* ``Params.peak_rel_threshold`` (0.45), -# which selects wedges to keep in the declutter reconstruction — a different job -# where being conservative is right. At 0.45, picking on the floor-subtracted -# residual drops a whole family on some rotations of an unchanged map and its -# energy lands in ``unassigned_energy_frac`` (measured 0.099 -> 0.181 -> 0.163 -# across 0/+17/-31 deg). At 0.15 the same map gives 0.099 / 0.071 / 0.084. +# reported against. It is the same 0.15 as ``Params.peak_rel_threshold`` because +# both now threshold the same *kind* of curve — a floor-subtracted residual, in +# which a family's height is its structural energy rather than its structural +# energy plus a broadband pedestal (see :func:`_pick_directions`). Measured here +# first: at 0.45 picking on the residual drops a whole family on some rotations +# of an unchanged map and its energy lands in ``unassigned_energy_frac`` +# (0.099 -> 0.181 -> 0.163 across 0/+17/-31 deg); at 0.15 the same map gives +# 0.099 / 0.071 / 0.084. STATS_PEAK_REL_THRESHOLD = 0.15 # At most this many direction families. ``Params.max_directions`` is 5, which @@ -206,15 +210,32 @@ def _smooth_circular(x: np.ndarray, k: int) -> np.ndarray: return np.convolve(padded, kernel, mode="valid") -def _pick_directions( - angles: np.ndarray, energy: np.ndarray, params: SpectrumParams +def _floor_subtract(energy: np.ndarray, params: SpectrumParams) -> np.ndarray: + """Angular energy with its broadband floor removed. + + The floor is the curve smoothed over :data:`FLOOR_HALFWIDTH_DEG`, i.e. what + is left of the energy once anything with a wall's angular sharpness is + averaged away. Subtracting it turns "how tall is this peak" into "how much + energy does this peak carry *above the background*", which is the quantity + every consumer here actually means. + """ + k = max(1, int(round(FLOOR_HALFWIDTH_DEG / params.angle_step_deg))) + return np.clip(energy - _smooth_circular(energy, k), 0.0, None) + + +def _pick_peaks( + angles: np.ndarray, curve: np.ndarray, params: SpectrumParams ) -> list[float]: - """Non-max-suppress the angular energy into a small set of orientations.""" - thresh = params.peak_rel_threshold * float(energy.max()) - order = np.argsort(-energy) + """Non-max-suppress a curve into a small set of orientations. + + ``curve`` must already be floor-subtracted — see :func:`_pick_directions`, + which is the entry point for a caller holding raw angular energy. + """ + thresh = params.peak_rel_threshold * float(curve.max()) + order = np.argsort(-curve) chosen: list[float] = [] for idx in order: - if energy[idx] < thresh: + if curve[idx] < thresh: break a = float(angles[idx]) if all(_angdist(a, c) >= params.peak_nms_deg for c in chosen): @@ -224,6 +245,59 @@ def _pick_directions( return sorted(chosen) +def _pick_directions( + angles: np.ndarray, energy: np.ndarray, params: SpectrumParams +) -> list[float]: + """Dominant orientations of a raw angular-energy curve. + + **The threshold is on the floor-subtracted residual, not on the raw + height**, and that is the whole content of this function. Clutter, speckle + and ragged edges put energy at *every* orientation, so a real map's angular + energy is a modest set of wall peaks riding on a broad pedestal — measured + at ~0.49 of the global maximum on the 2026-08-02 flat map. A relative-height + test therefore spends half its range on the pedestal: the weakest real + family there stands at 0.64 of the maximum and a phantom at 0.50, a ratio of + 1.3, and no threshold placed between two such numbers is a measurement. + Subtract the floor and the same two are 0.296 and 0.012 — a ratio of 24. + + The phantom is worth describing because it is the common case rather than a + curiosity: it is the *shoulder* of a real family, one NMS radius away + (13.0 deg from its parent at a 12 deg radius), so it survives suppression + and inherits most of its parent's flank height. Six of the thirteen real + occupancy maps on hand carried one, every one of them at 12.0-13.5 deg from + a stronger peak and with essentially zero prominence. + + Two things were tried first and are worse, both measured on that corpus: + + * **Literal topographic prominence** (height above the higher flanking + minimum) cannot express what is wanted here, at any threshold. It is what + ROSE itself uses — ``find_peaks(prominence=(max - min) * 0.5)`` in the + reference implementation — and at that threshold it is *more* conservative + than this: on all 13 maps it returns exactly the two strongest, + near-orthogonal directions and never a phantom. It also drops the real + off-axis families with them (11.8 and 110.2 deg on the flat map), which is + the two-direction result an operator rejected for visibly eroding walls. + Lower the threshold to keep those and the shoulder comes back with them: + the real off-axis family's prominence is 0.033 of the maximum against the + phantom's 0.028, since it sits on the dominant family's tail and is a + shoulder in exactly the same sense. Prominence also rewards isolation, so + a threshold low enough to admit them promotes lone bumps in the noise + floor carrying no structural energy at all (47.2 deg on the tuning map, + 43.2 and 137.2 deg on the replay maps). + * **A wider NMS radius** cannot separate them either: the phantom is 13.0 + deg from its parent and the real off-axis family is 14.5 deg from its own, + so any radius that suppresses the one suppresses the other. + + So this is a deliberate departure from ROSE, and the reason is the map: ROSE + scores large, overwhelmingly rectilinear building floor plans, where two + directions is the right answer and a third is usually clutter. A small flat + mapped by a 2-D lidar has genuine off-axis wall families, and the declutter + pass keeps *observed pixels* gated to real walls rather than abstracting a + floor plan, so a dropped family costs real wall rather than detail. + """ + return _pick_peaks(angles, _floor_subtract(energy, params), params) + + def _angdist(a: float, b: float) -> float: """Smallest distance between two orientations on the 180-deg circle.""" d = abs(a - b) % 180.0 @@ -308,8 +382,7 @@ def _residual_spectrum( signal = signal * np.hanning(h)[:, None] * np.hanning(w)[None, :] mag = np.abs(np.fft.fftshift(np.fft.fft2(signal))) angles, energy = _angular_energy(mag, params) - k = max(1, int(round(FLOOR_HALFWIDTH_DEG / params.angle_step_deg))) - residual = np.clip(energy - _smooth_circular(energy, k), 0.0, None) + residual = _floor_subtract(energy, params) total = float(residual.sum()) q = residual / total if total > 0 else residual return angles, energy, q @@ -640,7 +713,9 @@ def angular_stats( max_directions=max_directions, wedge_halfwidth_deg=params.wedge_halfwidth_deg, ) - dirs = _pick_directions(angles, q, pick) + # ``q`` is already the floor-subtracted residual, so this is the low-level + # picker rather than :func:`_pick_directions`, which would subtract twice. + dirs = _pick_peaks(angles, q, pick) directions = _directions_table(angles, q, params, dirs) frames, dominant_share = _frames_table(directions, frame_merge_deg) diff --git a/mote_bringup/mote_bringup/map_cleanup/cli.py b/mote_bringup/mote_bringup/map_cleanup/cli.py index 56a2906..b3c7d77 100644 --- a/mote_bringup/mote_bringup/map_cleanup/cli.py +++ b/mote_bringup/mote_bringup/map_cleanup/cli.py @@ -61,24 +61,41 @@ def _spectrum_panel(res: StructureResult, size: tuple[int, int]) -> np.ndarray: def _energy_panel(res: StructureResult, size: tuple[int, int]) -> np.ndarray: - """Angular-energy curve g(theta) with detected peaks marked.""" + """Angular-energy curve g(theta) with detected peaks marked. + + Both curves are drawn: the raw energy dim, and the floor-subtracted + residual — the one the picker thresholds — bright, with the threshold + across it. A panel showing only the raw curve cannot explain a rejection, + since a phantom direction is a visible bump there and a flat nothing here. + """ h, w = size img = np.full((h, w, 3), 30, np.uint8) - e = res.energy / (res.energy.max() + 1e-9) - n = len(e) - for i in range(n - 1): - x0 = int(i / n * (w - 1)) - x1 = int((i + 1) / n * (w - 1)) - y0 = int(h - 20 - e[i] * (h - 30)) - y1 = int(h - 20 - e[i + 1] * (h - 30)) - cv2.line(img, (x0, y0), (x1, y1), (200, 200, 60), 1, cv2.LINE_AA) + + def plot(curve, colour): + y = curve / (curve.max() + 1e-9) + n = len(y) + for i in range(n - 1): + cv2.line( + img, + (int(i / n * (w - 1)), int(h - 20 - y[i] * (h - 50))), + (int((i + 1) / n * (w - 1)), int(h - 20 - y[i + 1] * (h - 50))), + colour, + 1, + cv2.LINE_AA, + ) + + plot(res.energy, (90, 90, 40)) + if res.residual is not None and res.params is not None: + plot(res.residual, (200, 200, 60)) + y = int(h - 20 - res.params.peak_rel_threshold * (h - 50)) + cv2.line(img, (0, y), (w - 1, y), (90, 90, 220), 1) for d in res.directions_deg: x = int(d / 180.0 * (w - 1)) - cv2.line(img, (x, 10), (x, h - 20), (80, 255, 80), 1, cv2.LINE_AA) + cv2.line(img, (x, 20), (x, h - 20), (80, 255, 80), 1, cv2.LINE_AA) cv2.putText( img, f"{d:.0f}", - (min(x + 2, w - 22), 22), + (min(x + 2, w - 28), 32), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (80, 255, 80), @@ -87,7 +104,7 @@ def _energy_panel(res: StructureResult, size: tuple[int, int]) -> np.ndarray: ) cv2.putText( img, - "angular energy vs orientation (deg)", + "orientation (deg): raw energy (dim), above-floor residual + threshold", (4, h - 6), cv2.FONT_HERSHEY_SIMPLEX, 0.35, diff --git a/mote_bringup/mote_bringup/map_cleanup/room_segmentation.py b/mote_bringup/mote_bringup/map_cleanup/room_segmentation.py index a2a8888..463b446 100644 --- a/mote_bringup/mote_bringup/map_cleanup/room_segmentation.py +++ b/mote_bringup/mote_bringup/map_cleanup/room_segmentation.py @@ -48,7 +48,8 @@ UNKNOWN, Params, _angular_energy, - _pick_directions, + _floor_subtract, + _pick_peaks, ) @@ -140,13 +141,16 @@ def dominant_rotation_deg(occ: np.ndarray, params: Params | None = None) -> floa square[top : top + wall.shape[0], left : left + wall.shape[1]] = wall mag = np.abs(np.fft.fftshift(np.fft.fft2(square))) angles, energy = _angular_energy(mag, params) - directions = _pick_directions(angles, energy, params) + residual = _floor_subtract(energy, params) + directions = _pick_peaks(angles, residual, params) if not directions: return 0.0 - # Strongest direction first -- _pick_directions returns them sorted by - # angle, so re-rank by the energy at each. + # Strongest direction first -- _pick_peaks returns them sorted by angle, so + # re-rank by the energy at each. Ranking on the same floor-subtracted curve + # the peaks came from: on the raw one the tallest peak is whichever family + # sits highest on the pedestal, which is not the same question. idx = [int(np.argmin(np.abs(angles - d))) for d in directions] - best = directions[int(np.argmax([energy[i] for i in idx]))] + best = directions[int(np.argmax([residual[i] for i in idx]))] rot = best % 90.0 return rot - 90.0 if rot > 45.0 else rot diff --git a/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py b/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py index 9da45a5..0f34ad8 100644 --- a/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py +++ b/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py @@ -13,11 +13,18 @@ 1. binarise the occupancy grid into a wall image, 2. take its 2D FFT and measure spectral energy as a function of angle, - 3. pick the dominant orientations (local maxima of that angular energy), + 3. pick the dominant orientations (peaks of that angular energy, measured + above its broadband floor rather than above zero), 4. keep only the frequency wedges around those orientations (a directional band-pass) and invert the FFT to get a continuous "structure score", 5. threshold that score back into a decluttered occupancy grid. +Step 3 departs from ROSE on purpose: it picks peaks above the angular energy's +broadband floor, where ROSE thresholds their topographic prominence at half the +curve's range — which returns two directions on every map measured here, and +erodes the off-axis walls a small flat actually has. +:func:`angular_stats._pick_directions` carries the measurements. + Room segmentation (the ROSE2 layer on top of this) is intentionally out of scope for this module. @@ -33,7 +40,7 @@ import cv2 import numpy as np -from .angular_stats import _angular_energy, _pick_directions +from .angular_stats import _angular_energy, _floor_subtract, _pick_peaks # ROS map_server occupancy-PNG conventions. FREE = 254 @@ -50,18 +57,23 @@ class StructureResult: spectrum: np.ndarray # float, log magnitude spectrum (fftshifted) angles_deg: np.ndarray # angular-energy sample angles, [0, 180) energy: np.ndarray # angular energy per sample angle + residual: np.ndarray = None # energy with its broadband floor removed directions_deg: list[float] = field(default_factory=list) # detected peaks mask: np.ndarray = None # float, directional band-pass mask (fftshifted) score: np.ndarray = None # float in [0, 1], reconstructed structure score clean_wall: np.ndarray = None # bool, decluttered walls cleaned_map: np.ndarray = None # uint8, ROS occupancy PNG + params: "Params" = None # the settings that produced all of the above @dataclass class Params: angle_step_deg: float = 0.5 # angular resolution of the energy scan lowcut_frac: float = 0.02 # ignore this central disc of the spectrum (DC) - peak_rel_threshold: float = 0.45 # peak must reach this frac of the max + # Peak must reach this frac of the max *of the floor-subtracted residual*, + # which is a different and much wider scale than the raw energy this was + # once compared against — see angular_stats._pick_directions. + peak_rel_threshold: float = 0.15 peak_nms_deg: float = 12.0 # suppress peaks closer than this to a stronger one max_directions: int = 5 # keep at most this many orientations wedge_halfwidth_deg: float = 5.0 # angular half-width kept around each peak @@ -113,7 +125,8 @@ def extract_structure(occ: np.ndarray, params: Params | None = None) -> Structur spectrum_log = np.log1p(mag) angles, energy = _angular_energy(mag, params) - directions = _pick_directions(angles, energy, params) + residual = _floor_subtract(energy, params) + directions = _pick_peaks(angles, residual, params) mask = _directional_mask(signal.shape, directions, params) filtered = fft * mask @@ -162,11 +175,13 @@ def extract_structure(occ: np.ndarray, params: Params | None = None) -> Structur spectrum=spectrum_log, angles_deg=angles, energy=energy, + residual=residual, directions_deg=directions, mask=mask, score=score, clean_wall=clean, cleaned_map=cleaned_map, + params=params, ) diff --git a/mote_bringup/test/test_angular_stats.py b/mote_bringup/test/test_angular_stats.py index 27605bd..2768d3d 100755 --- a/mote_bringup/test/test_angular_stats.py +++ b/mote_bringup/test/test_angular_stats.py @@ -23,6 +23,11 @@ from mote_bringup.map_cleanup.angular_stats import ( # noqa: E402 SpectrumParams, + _angdist, + _angular_energy, + _floor_subtract, + _pick_directions, + _pick_peaks, angular_stats, fold_90, refine_peak, @@ -545,6 +550,104 @@ def test_score_is_invariant_to_incidental_map_extent(): assert abs(a["unassigned_energy_frac"] - b["unassigned_energy_frac"]) < 1e-6 +# -------------------------------------------------------------------------- +# Peak picking: what counts as a wall direction + + +OLD_RULE = SpectrumParams(peak_rel_threshold=0.45) # relative height, raw curve + + +def _declutter_curve(mask): + """The angular energy the declutter pass sees: no crop, no taper.""" + mag = np.abs(np.fft.fftshift(np.fft.fft2(mask.astype(np.float32)))) + return _angular_energy(mag, SpectrumParams()) + + +def _shoulder_curve(): + """A dominant family, a weaker real one, and a bump on the dominant's flank. + + Shaped after the 2026-08-02 flat map, where the phantom sat 13 deg from its + parent — just outside the 12 deg suppression radius — on a broadband + pedestal measuring ~0.49 of the global maximum. + """ + step = SpectrumParams().angle_step_deg + a = np.arange(0, 180, step) + step / 2 + + def gauss(centre, height, sigma): + d = np.abs(a - centre) % 180.0 + return height * np.exp(-0.5 * (np.minimum(d, 180.0 - d) / sigma) ** 2) + + curve = ( + 0.49 + + gauss(90.0, 0.51, 6.0) # dominant wall family + + gauss(20.0, 0.15, 6.0) # real, weaker, off-axis family + + gauss(103.0, 0.06, 2.5) # a bump on the dominant family's flank + ) + return a, curve + + +def test_a_shoulder_of_a_real_family_is_not_a_wall_direction(): + """The defect: a phantom direction 13 deg from its parent, over the gate. + + It is not that the shoulder is tall — it is that on a curve riding a + pedestal, *everything* is tall. Here it reaches 0.60 of the maximum against + the weakest real family's 0.64, so no relative-height threshold separates + them. Above the floor the same two are 0.06 and 0.30. + """ + angles, curve = _shoulder_curve() + + resid = _floor_subtract(curve, SpectrumParams()) + at = lambda c: int(np.argmin(np.abs(angles - c))) # noqa: E731 + assert curve[at(103.0)] / curve.max() > OLD_RULE.peak_rel_threshold + assert resid[at(103.0)] < 0.25 * resid[at(20.0)] + + picked = _pick_directions(angles, curve, SpectrumParams()) + assert [round(d) for d in picked] == [20, 90], picked + + # And what the shipped rule did with it, so the fixture is known to bite. + old = _pick_peaks(angles, curve, OLD_RULE) + assert any(_angdist(d, 103.0) <= 3.0 for d in old), old + + +def test_a_broadband_pedestal_does_not_change_the_picked_directions(): + """Clutter raises every orientation at once; that must not pick directions. + + A map's angular energy is wall peaks riding on whatever speckle, ragged + edges and furniture contribute at *all* orientations. Since the pedestal + carries no orientation, adding one must leave the answer alone — and under + a relative-height rule it does not: the threshold ends up below the + pedestal, every bin clears it, and the picker returns the five + highest bins the suppression radius allows rather than five wall families. + """ + angles, energy = _declutter_curve(pure(11.0)) + walls = _pick_directions(angles, energy, SpectrumParams()) + assert len(walls) == 2, walls + + cluttered = energy + 3.0 * energy.max() + assert _pick_directions(angles, cluttered, SpectrumParams()) == walls + assert len(_pick_peaks(angles, cluttered, OLD_RULE)) == 5 + + +def test_the_declutter_pass_picks_wall_families_not_flanks(): + """End to end through the shipped defaults, on a rectilinear map. + + This is what catches the dangerous half of the change being made alone: + dropping the threshold to 0.15 while still measuring the raw curve returns + four directions on this map, of which two are flank samples. + """ + from mote_bringup.map_cleanup.structure_extraction import ( + FREE, + OCCUPIED, + Params, + extract_structure, + ) + + occ = np.where(pure(11.0), OCCUPIED, FREE).astype(np.uint8) + res = extract_structure(occ, Params()) + assert len(res.directions_deg) == 2, res.directions_deg + assert abs(_angdist(*res.directions_deg) - 90.0) < 2.0, res.directions_deg + + def test_declutter_params_duck_type_in(): """The declutter pass passes its own Params straight through.""" from mote_bringup.map_cleanup.structure_extraction import Params