diff --git a/mote_bringup/mote_bringup/map_cleanup/angular_stats.py b/mote_bringup/mote_bringup/map_cleanup/angular_stats.py new file mode 100644 index 0000000..3b67795 --- /dev/null +++ b/mote_bringup/mote_bringup/map_cleanup/angular_stats.py @@ -0,0 +1,666 @@ +"""Angular structure of a map's walls, from the FFT orientation spectrum. + +The declutter pass (:mod:`structure_extraction`) already measures how a map's +Fourier energy distributes over wall orientations, in order to keep the wedges +around the dominant ones. This module reuses that same measurement to *score* +the map instead of filtering it: how many distinct directions the walls use, +how tightly the energy sits on them, and how those directions group into +orthogonal frames. + +It answers a failure the crispness proxies (wall thickness, speckle, unknown +fraction) are blind to: a section of the map drawn at the wrong *angle*. A +drift-rotated room is crisp and unspeckled and still wrong. + +What this can and cannot decide +------------------------------- +**Within a single map, a coherent drift-rotated section is angularly +indistinguishable from real architecture.** Both are a narrow extra family of +wall directions. A building with an angled hallway genuinely has three dominant +directions and always will; charging it for that would raise exactly the signal +this module exists to raise for drift, which is worse than no metric because it +would be believed. + +So the split is: + +* the scalars (``angular_support_deg``, ``angular_entropy_norm``, + ``unassigned_energy_frac``) are **frame-model-free** and describe angular + *smear and fragmentation*. They make no assumption about how many directions + a building has; +* the **defect verdict** needs a prior, and is only produced when one is given. + Pass ``reference_directions`` — the site's known wall directions, or those of + a previously banked revision — and the ``reference_*`` keys report how far + this map's energy sits from it. + +The frame table is the diagnostic between the two: a drift-rotated section of a +rectilinear building duplicates that section's whole orthogonal frame (both +directions, ~90 deg apart), where an angled hallway adds *one* direction. + +Two consumers, two entry points +------------------------------- +:func:`angular_stats` is the **tear alarm**. Loop drift only means anything when +the trajectory closes, so a mapping session that exits on its exploration budget +gives a scorer no drift number at all; for those the frame table is the only +automated tear signal there is. It is trustworthy for the tears that matter +(measured: run 3's two frames, 25 and 41 deg apart) and blind below roughly +its own merge tolerance — see :data:`FRAME_MERGE_DEG`. + +:func:`wall_rotation` is the **alignment primitive**: windowed energy, folded +0/90, sub-bin interpolated. An alignment step measuring how far a map's wall +grid is turned should call it rather than re-deriving the fold, which is how one +hand-rolled attempt landed on a different answer than the next. + +Conventions +----------- +Angles are in **image/pixel** coordinates (row-down), on [0, 180), and are not +map-frame bearings. A map frame's absolute rotation is an accident of where the +SLAM session started, so only *relative* angles here carry meaning; that is why +the reference comparison fits a global rotation offset before measuring. + +numpy-only and ROS-free by design: :mod:`metrics` imports this lazily so a +benchmark run in an environment without OpenCV, or without ``mote_bringup`` on +the path at all, still scores. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +__all__ = [ + "SpectrumParams", + "angular_stats", + "wall_rotation", + "spectral_to_wall", + "wall_to_spectral", + "fold_90", + "refine_peak", + "_angular_energy", + "_smooth_circular", + "_pick_directions", + "_angdist", +] + + +@dataclass +class SpectrumParams: + """The spectrum-scan half of :class:`structure_extraction.Params`. + + The declutter pass owns the full ``Params`` and passes it straight in — the + helpers below only read attributes, so it duck-types. This module carries + its own defaults instead of importing that one because the dependency has to + run the other way: ``structure_extraction`` imports cv2, and both + :mod:`metrics` and this module's callers need the angular maths to stay + numpy-only and cheap to import. Field names and defaults match ``Params``. + """ + + angle_step_deg: float = 0.5 + lowcut_frac: float = 0.02 + peak_rel_threshold: float = 0.45 + peak_nms_deg: float = 12.0 + max_directions: int = 5 + wedge_halfwidth_deg: float = 5.0 + + +# Half-width of the broadband floor estimated and subtracted from the angular +# energy, in degrees. Load-bearing: on an un-subtracted spectrum clutter +# dominates and the angular energy of a torn map is nearly uniform, so a torn +# map can score *better* than a clean one (measured, 2026-07-29). +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. +STATS_PEAK_REL_THRESHOLD = 0.15 + +# At most this many direction families. ``Params.max_directions`` is 5, which +# censors the table: every real map measured hits that cap. +STATS_MAX_DIRECTIONS = 12 + +# Directions merge into one orthogonal frame when their ``angle mod 90`` values +# are within this. Bounded from below by the shear a genuine frame carries (the +# run-3 conservative leg's own frame is internally sheared 7.5 deg, and splitting +# that into two frames would invent a tear), and from above by the smallest tear +# worth resolving. Measured on the synthetic fixtures: at 10 deg a 13 deg rotated +# section reads as a second frame of 2 directions while an angled corridor stays +# 1, and at 12 deg the rotated section's near wall family merges back into the +# dominant frame and the two become indistinguishable. Run 3's real 23-38 deg +# tear reads identically at either. +FRAME_MERGE_DEG = 10.0 + +# Zero-padding added on each side before the Hann taper in `wall_rotation`, as a +# fraction of the (already squared) extent, so the taper's roll-off lands on +# empty space instead of eating the walls. Measured on synthetic rotated room +# outlines, mean / max error over six rotations of 3 deg and up: +# 0.0 -> 0.215/1.062 deg, 0.25 -> 0.226/1.053, 0.5 -> 0.068/0.102. Costs +# (1+2f)^2 in transform area. +ROTATION_PAD_FRAC = 0.5 + +# A wall's Fourier energy lies on the ridge *perpendicular* to it (a horizontal +# line transforms to a vertical frequency ridge), so a peak in the angular +# spectrum sits at the wall's normal, not its orientation. Everything internal +# works in that spectral frame; angles are converted at the reporting boundary +# so that a table labelled "wall directions" contains wall directions. +WALL_NORMAL_OFFSET_DEG = 90.0 + + +def spectral_to_wall(angle_deg: float) -> float: + """Spectrum peak angle -> the orientation of the wall that produced it.""" + return float((angle_deg + WALL_NORMAL_OFFSET_DEG) % 180.0) + + +def wall_to_spectral(angle_deg: float) -> float: + """The inverse: a real wall orientation -> where its energy lands.""" + return float((angle_deg - WALL_NORMAL_OFFSET_DEG) % 180.0) + + +def _angular_energy( + spectrum_lin: np.ndarray, params: SpectrumParams +) -> tuple[np.ndarray, np.ndarray]: + """Sum spectral energy into angle bins over [0, 180). + + Each spectrum pixel at (u, v) relative to the centre contributes to the + orientation atan2(v, u) (mod 180, since the magnitude spectrum is + centro-symmetric). A wall puts its energy on the line through the origin + *perpendicular* to itself -- a horizontal line transforms to a vertical + frequency ridge -- so a peak here is a wall **normal**. Callers reporting a + real angle must convert with :func:`spectral_to_wall`; callers that put + something back into this same index space (the declutter pass's wedges) must + not. + """ + h, w = spectrum_lin.shape + cy, cx = h / 2.0, w / 2.0 + yy, xx = np.mgrid[0:h, 0:w] + dy = yy - cy + dx = xx - cx + radius = np.hypot(dx, dy) + rmax = min(cy, cx) + + # Drop the DC neighbourhood and the extreme high-frequency corners. + keep = (radius > params.lowcut_frac * rmax) & (radius <= rmax) + + ang = np.degrees(np.arctan2(dy, dx)) % 180.0 + nbins = int(round(180.0 / params.angle_step_deg)) + bin_idx = np.clip((ang / 180.0 * nbins).astype(int), 0, nbins - 1) + + weights = np.where(keep, spectrum_lin, 0.0).ravel() + energy = np.bincount(bin_idx.ravel(), weights=weights, minlength=nbins) + # Normalise out the fact that low-angle-resolution bins near the axes hold + # different pixel counts is negligible here; a light smoothing stabilises + # peak-picking on small maps. + energy = _smooth_circular(energy, k=max(1, int(round(2.0 / params.angle_step_deg)))) + angles = (np.arange(nbins) + 0.5) * params.angle_step_deg + return angles, energy + + +def _smooth_circular(x: np.ndarray, k: int) -> np.ndarray: + """Circular moving-average smoothing (angles wrap at 180 deg).""" + if k <= 1: + return x + kernel = np.ones(2 * k + 1) / (2 * k + 1) + padded = np.concatenate([x[-k:], x, x[:k]]) + return np.convolve(padded, kernel, mode="valid") + + +def _pick_directions( + angles: np.ndarray, energy: 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) + chosen: list[float] = [] + for idx in order: + if energy[idx] < thresh: + break + a = float(angles[idx]) + if all(_angdist(a, c) >= params.peak_nms_deg for c in chosen): + chosen.append(a) + if len(chosen) >= params.max_directions: + break + return sorted(chosen) + + +def _angdist(a: float, b: float) -> float: + """Smallest distance between two orientations on the 180-deg circle.""" + d = abs(a - b) % 180.0 + return min(d, 180.0 - d) + + +def _angdist_arr(a: np.ndarray, b: float) -> np.ndarray: + """Vectorised :func:`_angdist` for an array of orientations against one.""" + d = np.abs(a - b) % 180.0 + return np.minimum(d, 180.0 - d) + + +def _crop_to_content(wall: np.ndarray) -> np.ndarray: + """Crop to the bounding box of the wall cells. + + An incidental map extent — how much never-observed padding the grid carries + — must not change the score, so the transform runs over content only. The + public entry point takes a wall mask alone (the two callers hold different + occupancy conventions), so the wall bounding box is the available proxy for + the decided region; it is contained in it, which makes this the stricter + crop of the two. + """ + rows = np.any(wall, axis=1) + cols = np.any(wall, axis=0) + if not rows.any(): + return wall + r0, r1 = np.where(rows)[0][[0, -1]] + c0, c1 = np.where(cols)[0][[0, -1]] + return _pad_to_square(wall[r0 : r1 + 1, c0 : c1 + 1]) + + +def _pad_to_square(mask: np.ndarray) -> np.ndarray: + """Centre a mask in a square canvas before it is transformed. + + **Load-bearing, and the failure it prevents is silent.** The angular scan + measures orientation in *array index* space, and a frequency-domain index + maps to a real frequency divided by that axis' length — so on an oblong + array the two axes carry different scales and every angle is skewed towards + the long one. A pair of walls that really are perpendicular then stops + looking perpendicular: at bbox aspect 0.8 a 45-deg-rotated orthogonal frame + splits by 12.7 deg, at 0.66 by 23.2 deg, at 0.5 by 36.9 deg — all past + :data:`FRAME_MERGE_DEG`, so :func:`_frames_table` reports two frames and an + intact building reads as torn. + + Measured on an elongated but perfectly rectilinear building: rotated 10, 20 + and 30 deg it reported 2 frames un-padded, and 1 padded. Note *which* maps + it spares — 0 and 90 deg are fixed points of the distortion, so an + axis-aligned map is unaffected and the bug only appears once the map frame + is rotated, which a real SLAM frame always is. + + ``room_segmentation.py`` already does this, for this reason, having measured + an 8 deg skew on a 58 x 38 m map. The declutter pass genuinely does not need + it — it places its wedges back in the same index space it found them in — + but every angle *reported* from here is meant to be a real one. + """ + h, w = mask.shape + if h == w: + return mask + side = max(h, w) + out = np.zeros((side, side), dtype=mask.dtype) + top, left = (side - h) // 2, (side - w) // 2 + out[top : top + h, left : left + w] = mask + return out + + +def _residual_spectrum( + wall: np.ndarray, params: SpectrumParams, window: bool = False +) -> tuple: + """Angular energy with its broadband floor removed, normalised to sum 1.""" + signal = wall.astype(np.float32) + if window: + # Pad before tapering. A Hann window over a *tight* crop cuts into the + # walls themselves -- worst of all on a rotated shape, whose corners + # reach the crop edge -- and measurably costs accuracy: on synthetic + # rotated room outlines, crop+taper gives 0.68 deg mean rotation error + # against 0.39 deg for no taper at all. Padding puts the taper's roll-off + # on empty space where it belongs, giving 0.14 deg. + pad = ROTATION_PAD_FRAC + py, px = (int(round(s * pad)) for s in signal.shape) + signal = np.pad(signal, ((py, py), (px, px))) + h, w = signal.shape + 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) + total = float(residual.sum()) + q = residual / total if total > 0 else residual + return angles, energy, q + + +def fold_90(angles: np.ndarray, values: np.ndarray) -> tuple: + """Fold an orientation distribution from [0, 180) onto [0, 90). + + An orthogonal frame's two wall families sit 90 deg apart, so folding puts + both on one peak: the quantity that survives is the frame's *rotation*, + which is what an alignment step wants to measure and correct. + """ + n = len(values) // 2 + return angles[:n], values[:n] + values[n : 2 * n] + + +def _parabolic_offset(y0: float, y1: float, y2: float) -> float: + """Sub-bin peak offset in bins, from a parabola through three samples. + + Returns 0 for a flat or inverted triple rather than dividing by zero, and is + clamped to the sampled interval so a near-degenerate fit cannot throw the + peak into a neighbouring bin. + """ + den = y0 - 2.0 * y1 + y2 + if den == 0 or not np.isfinite(den): + return 0.0 + return float(np.clip(0.5 * (y0 - y2) / den, -0.5, 0.5)) + + +def refine_peak(angles: np.ndarray, values: np.ndarray, index: int) -> float: + """Sub-bin interpolated angle of the peak at ``index`` (circular).""" + n = len(values) + step = float(angles[1] - angles[0]) if n > 1 else 0.0 + offset = _parabolic_offset( + float(values[(index - 1) % n]), + float(values[index]), + float(values[(index + 1) % n]), + ) + return float(angles[index] + offset * step) + + +def wall_rotation( + wall: np.ndarray, + params: SpectrumParams | None = None, + *, + window: bool = True, +) -> dict: + """Dominant wall rotation of a map, folded to [0, 90), with sub-bin precision. + + This is the canonical measurement an alignment step should use to answer + "how far is this map's wall grid turned?" before re-solving. It exists here, + rather than being hand-rolled at each call site, because the same fold was + written twice during the 2026-08-02 session and got a different answer each + time. + + ``window`` zero-pads by :data:`ROTATION_PAD_FRAC` and applies a 2-D Hann + taper, which removes the rectangular aperture's axis-aligned sinc cross. + Two things here were measured rather than assumed. + + **The aperture does not pin this fold to 0 deg**, with or without the taper: + on a room outline rotated 31 deg the peak lands at 59.25 deg un-tapered and + 58.75 deg tapered (truth 59.0), and the energy within 2 deg of 0/90 is + 0.026-0.030 in every combination of taper and low-cut. + ``_angular_energy`` already drops the DC neighbourhood and works on + magnitude rather than power, which is most likely why a hand-rolled fold + that skipped those steps behaved differently. This is pinned by a test. + + **The padding is not decoration.** Tapering a *tight* crop lets the + roll-off cut into the walls themselves, and a rotated shape's corners reach + the crop edge: 0.215 deg mean / 1.062 deg worst un-padded against + 0.068/0.102 padded. + + Returns ``angle_deg`` (sub-bin refined, in [0, 90)), ``bin_angle_deg`` (the + unrefined bin centre, for debugging), ``energy_frac`` (share of the folded + residual within ``wedge_halfwidth_deg`` of the peak — low means there is no + single dominant grid to align to) and ``windowed``. + + **There is a hard floor at about 2 deg, and it under-reports below it.** + A wall line rotated by less than ~2 deg on a pixel grid rasterises into runs + long enough that its dominant spectral content is still axis-aligned, so the + peak is pulled onto the axis. Measured (true -> reported): -0.5 -> -0.13, + -1.0 -> -0.46, -1.5 -> -0.46, -2.0 -> -2.26, -3.0 -> -2.90, and 0.068 deg + mean error from 3 deg up. A bigger building does not help — it is + rasterisation, not resolution. + + So: good for measuring a wall grid's rotation and driving a re-solve at 2 + deg and above, and **unusable for the 1-2 deg residual shear** — it will + report roughly a third of it and a correction built on that will silently + under-correct. + """ + params = params or SpectrumParams() + wall = np.asarray(wall, dtype=bool) + if wall.ndim != 2 or wall.size == 0 or not wall.any(): + return {"angle_deg": None, "note": "no walls"} + + cropped = _crop_to_content(wall) + if min(cropped.shape) < 4: + return {"angle_deg": None, "note": "wall extent too small"} + + angles, _energy, q = _residual_spectrum(cropped, params, window=window) + if q.sum() <= 0: + return {"angle_deg": None, "note": "no angular structure"} + + fa, fq = fold_90(angles, q) + idx = int(np.argmax(fq)) + refined = refine_peak(fa, fq, idx) % 90.0 + + dist = np.abs(fa - refined) % 90.0 + dist = np.minimum(dist, 90.0 - dist) + total = float(fq.sum()) + return { + "angle_deg": refined, + "bin_angle_deg": float(fa[idx]), + "energy_frac": float(fq[dist <= params.wedge_halfwidth_deg].sum() / total) + if total > 0 + else 0.0, + "windowed": bool(window), + } + + +def _directions_table( + angles: np.ndarray, q: np.ndarray, params: SpectrumParams, dirs: list[float] +) -> list[dict]: + """Per-direction energy share and angular width about the family centre.""" + table = [] + for d in dirs: + dist = _angdist_arr(angles, d) + near = dist <= params.wedge_halfwidth_deg + share = float(q[near].sum()) + width = ( + float(np.sqrt(np.sum(q[near] * dist[near] ** 2) / share)) + if share > 0 + else 0.0 + ) + table.append( + { + "angle_deg": spectral_to_wall(d), + "energy_frac": share, + "width_deg": width, + } + ) + return table + + +def _frames_table(directions: list[dict], merge_deg: float) -> tuple[list[dict], float]: + """Group directions into orthogonal frames on ``angle mod 90``. + + A rectilinear building contributes both of its wall directions to one + frame; an extra frame means a second rectilinear system — which is what a + drift-rotated *section* of the map looks like, as against an angled hallway, + which adds a single direction to an existing frame. + """ + if not directions: + return [], 0.0 + + ordered = sorted(directions, key=lambda d: -d["energy_frac"]) + frames: list[dict] = [] + for d in ordered: + a90 = d["angle_deg"] % 90.0 + for f in frames: + gap = abs(a90 - f["_a90"]) % 90.0 + if min(gap, 90.0 - gap) <= merge_deg: + f["_members"].append(d) + f["energy_frac"] += d["energy_frac"] + break + else: + frames.append( + {"_a90": a90, "_members": [d], "energy_frac": d["energy_frac"]} + ) + + frames.sort(key=lambda f: -f["energy_frac"]) + dominant = frames[0]["_a90"] + out = [] + for f in frames: + gap = abs(f["_a90"] - dominant) % 90.0 + out.append( + { + "angle_deg": float(f["_a90"]), + "energy_frac": float(f["energy_frac"]), + "n_directions": len(f["_members"]), + "offset_from_dominant_deg": float(min(gap, 90.0 - gap)), + } + ) + return out, float(frames[0]["energy_frac"]) + + +def _manhattan(angles: np.ndarray, q: np.ndarray, params: SpectrumParams) -> dict: + """Fit one orthogonal frame to the whole spectrum. + + Descriptive only, and **not rotation-stable on a multi-frame map**: the + fitted frame jumps between competing wall families, so the same unchanged + map scores differently after a global rotation (measured: share 0.778 -> + 0.625 -> 0.692 over 0/+17/-31 deg). Kept because it is a compact summary of + "how Manhattan is this map", not because it ranks anything. + """ + z = complex(np.sum(q * np.exp(4j * np.radians(angles)))) + frame = float((np.degrees(np.angle(z)) / 4.0) % 90.0) if abs(z) > 0 else 0.0 + gap = np.abs(angles - frame) % 90.0 + dist = np.minimum(gap, 90.0 - gap) + return { + "manhattan_frame_deg": frame, + "manhattan_concentration": float(abs(z)), + "manhattan_share": float(q[dist <= params.wedge_halfwidth_deg].sum()), + } + + +def _reference_fit( + angles: np.ndarray, + q: np.ndarray, + params: SpectrumParams, + reference_directions: list[float], +) -> dict: + """Align a reference direction set to this map and measure what misses it. + + A map frame's absolute rotation is arbitrary, so the reference set is free + to rotate as a whole: one global offset is searched over the bin grid, + minimising the energy-weighted angular distance from the spectrum to the + nearest reference direction. + """ + refs = np.asarray([wall_to_spectral(float(r)) for r in reference_directions]) + if refs.size == 0: + return {} + + # dist[i, j] = distance from bin j to reference i, before any offset. + per_ref = np.stack([_angdist_arr(angles, r) for r in refs]) + + best = None + for shift in range(len(angles)): + # Rotating the reference set by +shift bins is a circular shift of the + # per-bin distances in the opposite direction. + d = np.min(np.roll(per_ref, shift, axis=1), axis=0) + cost = float(np.sum(q * d**2)) + if best is None or cost < best[0]: + best = (cost, shift, d) + + cost, shift, dist = best + offset = (shift * params.angle_step_deg + 90.0) % 180.0 - 90.0 + return { + "reference_offset_deg": float(offset), + "off_reference_energy_frac": float(q[dist > params.wedge_halfwidth_deg].sum()), + "reference_dispersion_deg": float(np.sqrt(cost)), + } + + +def angular_stats( + wall: np.ndarray, + params: SpectrumParams | None = None, + reference_directions: list[float] | None = None, + *, + frame_merge_deg: float = FRAME_MERGE_DEG, + peak_rel_threshold: float = STATS_PEAK_REL_THRESHOLD, + max_directions: int = STATS_MAX_DIRECTIONS, +) -> dict: + """Score the angular structure of a boolean wall mask. + + ``wall`` is a 2-D boolean array, True where the map holds a wall. It is a + mask rather than a map image because the two callers hold different + occupancy conventions and neither should be converted lossily: the declutter + pass binarises a ROS occupancy PNG (0 occupied / 205 unknown / 254 free), + while the bag-replay scorer holds an ``OccupancyGrid`` array (-1 unknown, + 0..100) and thresholds it directly. + + Returns a dict with, in order of what it is for: + + **Ranked scalars** — frame-model-free, rotation-invariant by construction + (a global rotation is a circular shift of the spectrum, and none of these + depend on where the shift starts): + + * ``angular_support_deg`` — ``exp(H) * angle_step_deg``, the effective + number of degrees of wall direction the map uses. **Lower is better.** + * ``angular_entropy_norm`` — ``H / ln(nbins)``, the same in [0, 1]. + **Lower is better.** + * ``unassigned_energy_frac`` — energy further than ``wedge_halfwidth_deg`` + from *every* detected direction: genuine angular smear, sitting between + families rather than on one. **Lower is better.** Note what it does *not* + catch: a coherent rotated section forms its own family and so does not + raise this at all. That is what ``reference_directions`` is for. + + **Structure, not score** — ``directions`` (each ``angle_deg``, + ``energy_frac``, ``width_deg``), ``frames`` (each ``angle_deg``, + ``energy_frac``, ``n_directions``, ``offset_from_dominant_deg``) and + ``dominant_frame_share``. ``n_peaks`` is reported but is threshold-bound and + is not a score. + + **Verdict, when a prior is given** — pass ``reference_directions`` (the + site's known wall directions, in the same image convention; for a building + with an angled hallway the set *must include the hallway direction*, or the + metric re-acquires the single-frame bug this module exists to avoid) and get + ``reference_offset_deg``, ``off_reference_energy_frac`` and + ``reference_dispersion_deg``. + + ``manhattan_*`` is an unranked descriptor; see :func:`_manhattan` for why it + must not be ranked. + + The frame merge tolerance is the same order as the section rotations this + exists to catch — it must exceed the shear a real frame carries (7.5 deg + measured) yet stay under the tear being resolved — so the frame table is a + diagnostic to read, not a threshold to trip. Below roughly its own tolerance + a rotated section merges back into the dominant frame and becomes invisible + to it; ``reference_directions`` is what convicts at that scale. + + ``reference_offset_deg`` is unique only up to the reference set's own + symmetry: a rectilinear set repeats every 90 deg, so an offset of 84.5 and + one of -5.5 describe the same fit. + """ + params = params or SpectrumParams() + wall = np.asarray(wall, dtype=bool) + + if wall.ndim != 2 or wall.size == 0 or not wall.any(): + return {"n_peaks": 0, "note": "no walls"} + + cropped = _crop_to_content(wall) + if min(cropped.shape) < 4: + return {"n_peaks": 0, "note": "wall extent too small"} + + angles, _energy, q = _residual_spectrum(cropped, params) + if q.sum() <= 0: + return {"n_peaks": 0, "note": "no angular structure"} + + nz = q > 0 + entropy = float(-np.sum(q[nz] * np.log(q[nz]))) + + pick = SpectrumParams( + angle_step_deg=params.angle_step_deg, + lowcut_frac=params.lowcut_frac, + peak_rel_threshold=peak_rel_threshold, + peak_nms_deg=params.peak_nms_deg, + max_directions=max_directions, + wedge_halfwidth_deg=params.wedge_halfwidth_deg, + ) + dirs = _pick_directions(angles, q, pick) + directions = _directions_table(angles, q, params, dirs) + frames, dominant_share = _frames_table(directions, frame_merge_deg) + + nearest = np.min(np.stack([_angdist_arr(angles, d) for d in dirs]), axis=0) + + out = { + "angular_support_deg": float(np.exp(entropy) * params.angle_step_deg), + "angular_entropy_norm": float(entropy / np.log(len(q))), + "unassigned_energy_frac": float(q[nearest > params.wedge_halfwidth_deg].sum()), + "n_peaks": len(dirs), + "directions": directions, + "frames": frames, + "n_frames": len(frames), + # Frames carrying real energy - the tear signal. A rectilinear building + # is 1; a drift-rotated section makes 2. The share floor keeps a stray + # sliver of a family from reading as a second building. + "n_strong_frames": sum(1 for f in frames if f["energy_frac"] >= 0.15), + "dominant_frame_share": dominant_share, + } + out.update(_manhattan(angles, q, params)) + if reference_directions: + out.update(_reference_fit(angles, q, params, reference_directions)) + return out diff --git a/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py b/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py index 3498acb..9da45a5 100644 --- a/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py +++ b/mote_bringup/mote_bringup/map_cleanup/structure_extraction.py @@ -20,6 +20,10 @@ Room segmentation (the ROSE2 layer on top of this) is intentionally out of scope for this module. + +Steps 2 and 3 — the angular spectrum and its peak-picking — live in +:mod:`angular_stats`, which is numpy-only so that scoring a map's angular +structure does not drag OpenCV in. This module keeps the filtering half. """ from __future__ import annotations @@ -29,6 +33,8 @@ import cv2 import numpy as np +from .angular_stats import _angular_energy, _pick_directions + # ROS map_server occupancy-PNG conventions. FREE = 254 UNKNOWN = 205 @@ -73,75 +79,6 @@ def _binarise(occ: np.ndarray) -> tuple[np.ndarray, np.ndarray]: return wall, free -def _angular_energy( - spectrum_lin: np.ndarray, params: Params -) -> tuple[np.ndarray, np.ndarray]: - """Sum spectral energy into angle bins over [0, 180). - - Each spectrum pixel at (u, v) relative to the centre contributes to the - orientation atan2(v, u) (mod 180, since the magnitude spectrum is - centro-symmetric). A wall oriented at angle a puts its energy on the line - through the origin at that same angle, so peaks in this histogram are the - map's dominant orientations. - """ - h, w = spectrum_lin.shape - cy, cx = h / 2.0, w / 2.0 - yy, xx = np.mgrid[0:h, 0:w] - dy = yy - cy - dx = xx - cx - radius = np.hypot(dx, dy) - rmax = min(cy, cx) - - # Drop the DC neighbourhood and the extreme high-frequency corners. - keep = (radius > params.lowcut_frac * rmax) & (radius <= rmax) - - ang = np.degrees(np.arctan2(dy, dx)) % 180.0 - nbins = int(round(180.0 / params.angle_step_deg)) - bin_idx = np.clip((ang / 180.0 * nbins).astype(int), 0, nbins - 1) - - weights = np.where(keep, spectrum_lin, 0.0).ravel() - energy = np.bincount(bin_idx.ravel(), weights=weights, minlength=nbins) - # Normalise out the fact that low-angle-resolution bins near the axes hold - # different pixel counts is negligible here; a light smoothing stabilises - # peak-picking on small maps. - energy = _smooth_circular(energy, k=max(1, int(round(2.0 / params.angle_step_deg)))) - angles = (np.arange(nbins) + 0.5) * params.angle_step_deg - return angles, energy - - -def _smooth_circular(x: np.ndarray, k: int) -> np.ndarray: - """Circular moving-average smoothing (angles wrap at 180 deg).""" - if k <= 1: - return x - kernel = np.ones(2 * k + 1) / (2 * k + 1) - padded = np.concatenate([x[-k:], x, x[:k]]) - return np.convolve(padded, kernel, mode="valid") - - -def _pick_directions( - angles: np.ndarray, energy: np.ndarray, params: Params -) -> 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) - chosen: list[float] = [] - for idx in order: - if energy[idx] < thresh: - break - a = float(angles[idx]) - if all(_angdist(a, c) >= params.peak_nms_deg for c in chosen): - chosen.append(a) - if len(chosen) >= params.max_directions: - break - return sorted(chosen) - - -def _angdist(a: float, b: float) -> float: - """Smallest distance between two orientations on the 180-deg circle.""" - d = abs(a - b) % 180.0 - return min(d, 180.0 - d) - - def _directional_mask( shape: tuple[int, int], directions_deg: list[float], params: Params ) -> np.ndarray: diff --git a/mote_bringup/test/test_angular_stats.py b/mote_bringup/test/test_angular_stats.py new file mode 100755 index 0000000..27605bd --- /dev/null +++ b/mote_bringup/test/test_angular_stats.py @@ -0,0 +1,577 @@ +#!/usr/bin/env python3 +"""Angular-structure scoring, on synthetic wall masks only (no data files). + +The fixtures encode the design's central claim: a building with an angled +hallway has three genuine wall directions and must not be scored as broken for +it, while a drift-rotated *section* of a rectilinear building — which duplicates +that section's whole orthogonal frame — must be separable from it. + +Masks are drawn analytically at each angle rather than by rotating a raster, so +a rotation test measures the metric and not the resampler. Residual movement +under rotation is the staircase of a rasterised diagonal line; the pinned +tolerances are set from the measured values with margin, and are stated in the +assertions rather than hidden in a helper. +""" + +import sys +from pathlib import Path + +import numpy as np +import pytest + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from mote_bringup.map_cleanup.angular_stats import ( # noqa: E402 + SpectrumParams, + angular_stats, + fold_90, + refine_peak, + spectral_to_wall, + wall_rotation, + wall_to_spectral, +) + +N = 600 + +# Axis-aligned rooms: (centre_y, centre_x, height, width), in a 600 px canvas. +ROOMS = [ + (130, 130, 155, 185), + (130, 355, 155, 215), + (340, 130, 170, 185), + (340, 370, 170, 240), + (500, 240, 115, 285), +] + + +def _line(mask, p0, p1): + n = int(max(abs(p1[0] - p0[0]), abs(p1[1] - p0[1]))) * 3 + 2 + iy = np.rint(np.linspace(p0[0], p1[0], n)).astype(int) + ix = np.rint(np.linspace(p0[1], p1[1], n)).astype(int) + ok = (iy >= 0) & (iy < mask.shape[0]) & (ix >= 0) & (ix < mask.shape[1]) + mask[iy[ok], ix[ok]] = True + + +def _rect(mask, cy, cx, h, w, deg=0.0): + t = np.radians(deg) + c, s = np.cos(t), np.sin(t) + pts = [ + (cy + dy * c - dx * s, cx + dy * s + dx * c) + for dy, dx in ( + (-h / 2, -w / 2), + (-h / 2, w / 2), + (h / 2, w / 2), + (h / 2, -w / 2), + ) + ] + for i in range(4): + _line(mask, pts[i], pts[(i + 1) % 4]) + + +def pure(deg=0.0): + """A rectilinear room set, optionally rotated as a whole.""" + m = np.zeros((N, N), bool) + t = np.radians(deg) + c, s = np.cos(t), np.sin(t) + for cy, cx, h, w in ROOMS: + dy, dx = cy - N / 2, cx - N / 2 + _rect(m, N / 2 + dy * c - dx * s, N / 2 + dy * s + dx * c, h, w, deg) + return m + + +def angled_corridor(deg=20.0): + """The operator's case: the same rooms plus a hallway at an angle. + + Drawn as two parallel walls with no end caps, which is what a hallway is — + it opens at both ends — so it contributes exactly *one* extra direction. + """ + m = pure() + t = np.radians(deg) + c, s = np.cos(t), np.sin(t) + cy, cx, length = 305, 300, 430 + for off in (-19.0, 19.0): + _line( + m, + (cy + off * c + length / 2 * s, cx + off * s - length / 2 * c), + (cy + off * c - length / 2 * s, cx + off * s + length / 2 * c), + ) + return m + + +def rotated_rooms(deg=13.0): + """A drift-rotated section: a subset of rooms carrying its own frame.""" + m = np.zeros((N, N), bool) + for i, (cy, cx, h, w) in enumerate(ROOMS): + _rect(m, cy, cx, h, w, deg if i >= 3 else 0.0) + return m + + +def _dirs(stats): + return [d["angle_deg"] for d in stats["directions"]] + + +def _shift(a, b): + """Signed-free angular movement between two orientations, on [0, 90].""" + d = abs(a - b) % 180.0 + return min(d, 180.0 - d) + + +# -------------------------------------------------------------------------- +# The ranked scalars + + +def test_rectilinear_map_is_angularly_tight(): + """A pure rectilinear map uses few directions and smears little.""" + s = angular_stats(pure()) + assert s["angular_support_deg"] < 25.0, s["angular_support_deg"] + assert s["angular_entropy_norm"] < 0.65, s["angular_entropy_norm"] + assert s["unassigned_energy_frac"] < 0.10, s["unassigned_energy_frac"] + # Two wall families, one orthogonal frame holding both. + assert s["n_peaks"] == 2, _dirs(s) + assert len(s["frames"]) == 1 + assert s["frames"][0]["n_directions"] == 2 + assert s["dominant_frame_share"] > 0.9 + + +def test_angled_corridor_is_not_scored_as_a_defect(): + """The regression that keeps the metric from calling the operator's flat broken. + + A third genuine wall direction must cost something (three families is more + angular support than two, by construction) but must not read as damage. + """ + base = angular_stats(pure()) + corr = angular_stats(angled_corridor()) + + # Support may rise, but nowhere near a defect's rise (see the next test, + # where a rotated section is pinned well above this bound). + assert corr["angular_support_deg"] < 1.30 * base["angular_support_deg"], ( + corr["angular_support_deg"], + base["angular_support_deg"], + ) + assert corr["angular_entropy_norm"] < 1.10 * base["angular_entropy_norm"] + # Smear must not rise at all: a coherent extra family is not smear. + assert corr["unassigned_energy_frac"] <= base["unassigned_energy_frac"] + 0.02 + + # It is one extra direction, in a frame of its own holding only itself. + assert corr["n_peaks"] == 3, _dirs(corr) + secondary = corr["frames"][1] + assert secondary["n_directions"] == 1, corr["frames"] + + +def test_rotated_section_separates_from_an_angled_corridor(): + """A duplicated frame is what a drift-rotated section looks like.""" + corr = angular_stats(angled_corridor()) + rot = angular_stats(rotated_rooms(13.0)) + + # The frame table is the discriminator: two directions ~90 deg apart in the + # secondary frame, against the corridor's one. + assert len(rot["frames"]) >= 2, rot["frames"] + assert rot["frames"][1]["n_directions"] >= 2, rot["frames"] + assert corr["frames"][1]["n_directions"] == 1, corr["frames"] + + # And it is angularly looser than the corridor on the ranked scalars. + assert rot["angular_support_deg"] > corr["angular_support_deg"] + + +@pytest.mark.parametrize("skew", [20.0, 25.0, 30.0, 40.0]) +def test_frame_table_is_trustworthy_for_the_tears_it_is_relied_on_for(skew): + """The tear alarm's working band. + + Where the trajectory does not close there is no loop-drift number, and this + table is the only automated tear signal. It is relied on for tears of about + 20 deg and up (run 3's real pair were 25 and 41 deg apart), so that band + is pinned rather than left to the 13 deg case alone. + """ + s = angular_stats(rotated_rooms(skew)) + assert len(s["frames"]) >= 2, s["frames"] + secondary = s["frames"][1] + assert secondary["n_directions"] >= 2, s["frames"] + assert secondary["energy_frac"] > 0.15, s["frames"] + # The reported offset is the tear angle, folded onto [0, 45]. + expected = skew % 90.0 + expected = min(expected, 90.0 - expected) + assert abs(secondary["offset_from_dominant_deg"] - expected) < 5.0, s["frames"] + + +def test_frame_table_is_blind_below_its_merge_tolerance(): + """The limit, pinned as a fact rather than left as a caveat in prose. + + Directions closer than the merge tolerance are one frame by construction -- + they have to be, or the shear a genuine frame carries (7.5 deg measured on a + real leg) would be reported as a tear. So a small rotation is invisible here + and something with a prior has to catch it. + """ + s = angular_stats(rotated_rooms(5.0)) + assert len(s["frames"]) == 1, s["frames"] + # ...and the scalars do still notice something changed, they just cannot say + # whether it is drift or architecture. + assert s["angular_support_deg"] > angular_stats(pure())["angular_support_deg"] + + +def test_reference_directions_convict_the_rotation_and_absolve_the_hallway(): + """The defect verdict needs a prior; with one, the two cases part decisively.""" + base = angular_stats(pure()) + ref = _dirs(base) # the site's known rectilinear directions + + clean = angular_stats(pure(), reference_directions=ref) + rot = angular_stats(rotated_rooms(13.0), reference_directions=ref) + # Declared without the hallway, the hallway itself reads as off-reference — + # which is precisely why the set must include it. + corr_undeclared = angular_stats(angled_corridor(), reference_directions=ref) + # The hallway is the direction the reference set does *not* already cover -- + # picked by distance rather than by position, since the table is sorted by + # angle and which slot it lands in is not stable. + hall = max( + _dirs(angular_stats(angled_corridor())), + key=lambda a: min(_shift(a, r) for r in ref), + ) + corr_declared = angular_stats(angled_corridor(), reference_directions=ref + [hall]) + + assert clean["off_reference_energy_frac"] < 0.10 + assert rot["off_reference_energy_frac"] > 0.4 + assert ( + rot["off_reference_energy_frac"] + > 2 * corr_undeclared["off_reference_energy_frac"] + ) + # Declaring the hallway is what makes the good map score as a good map. + assert corr_declared["off_reference_energy_frac"] < 0.10, corr_declared + + # A map matching its reference sits on it; a torn one does not. + assert clean["reference_dispersion_deg"] < rot["reference_dispersion_deg"] + + +def test_reference_set_is_free_to_rotate_as_a_whole(): + """A map frame's absolute rotation is arbitrary, so the fit absorbs it.""" + ref = _dirs(angular_stats(pure())) + turned = angular_stats(pure(23.0), reference_directions=ref) + assert turned["off_reference_energy_frac"] < 0.10, turned + + +# -------------------------------------------------------------------------- +# Rotation invariance + + +@pytest.mark.parametrize("deg", [17.0, -31.0]) +def test_ranked_scalars_survive_a_global_rotation(deg): + """The same building, turned: the ranked scalars must not move materially. + + Rotation is a circular shift of the angular spectrum, and none of the three + depend on where the shift starts. The residual is the staircase of drawing + diagonal lines on a pixel grid (measured within 6%; pinned at 10%). + """ + base = angular_stats(pure()) + turned = angular_stats(pure(deg)) + + # Rotating the building changes its bounding box, hence how much zero + # padding squaring adds, hence the spectrum's broadband floor -- so support + # moves more than entropy does. Measured over +17/-31/+23/+45: support + # 3.6-15.2%, entropy 1.0-4.6%. Pinned with margin; this is also why support + # is not a ranking column. + for key, tol in ( + ("angular_support_deg", 0.20), + ("angular_entropy_norm", 0.10), + ): + rel = abs(turned[key] - base[key]) / base[key] + assert rel < tol, (key, base[key], turned[key], rel) + assert abs(turned["unassigned_energy_frac"] - base["unassigned_energy_frac"]) < 0.05 + + +@pytest.mark.parametrize("deg", [17.0, -31.0]) +def test_direction_table_tracks_a_global_rotation(deg): + """The families are the same families, moved by the rotation.""" + base = _dirs(angular_stats(pure())) + turned = _dirs(angular_stats(pure(deg))) + assert len(turned) == len(base) + + expected = abs(deg) % 90.0 + expected = min(expected, 90.0 - expected) + for a in turned: + moved = min(_shift(a, b) for b in base) + assert abs(moved - expected) < 4.0, (deg, base, turned, a, moved) + + +# -------------------------------------------------------------------------- +# The alignment primitive: windowed, folded 0/90, sub-bin interpolated + + +def _room_outline(deg, n=400, half_y=120, half_x=150, thick=3): + """A single rotated rectangular room outline - a clean rotation target.""" + yy, xx = np.mgrid[0:n, 0:n] + t = np.radians(deg) + c, s = np.cos(t), np.sin(t) + y = (yy - n / 2) * c + (xx - n / 2) * s + x = -(yy - n / 2) * s + (xx - n / 2) * c + outer = (np.abs(y) < half_y) & (np.abs(x) < half_x) + inner = (np.abs(y) < half_y - thick) & (np.abs(x) < half_x - thick) + return outer & ~inner + + +@pytest.mark.parametrize("deg", [0.0, 3.0, 7.25, 17.0, 23.5, 31.0, 44.0]) +def test_wall_rotation_recovers_a_known_rotation(deg): + r = wall_rotation(_room_outline(deg)) + truth = (-deg) % 90.0 + err = abs(r["angle_deg"] - truth) % 90.0 + err = min(err, 90.0 - err) + assert err < 0.5, (deg, truth, r) + assert r["energy_frac"] > 0.3, r + + +def test_sub_bin_refinement_beats_the_bin_grid(): + """Why the interpolation is there: 0.5 deg bins alone are not enough.""" + raw, refined = [], [] + for deg in (3.0, 7.25, 12.0, 23.5, 31.0, 44.0): + r = wall_rotation(_room_outline(deg)) + truth = (-deg) % 90.0 + for value, into in ((r["bin_angle_deg"], raw), (r["angle_deg"], refined)): + e = abs(value - truth) % 90.0 + into.append(min(e, 90.0 - e)) + assert np.mean(refined) < np.mean(raw), (np.mean(raw), np.mean(refined)) + # Good enough to measure a wall grid, not to certify a 1-2 deg shear absent. + # Measured 0.068 deg mean / 0.102 worst, against 0.208/0.250 unrefined. + assert np.mean(refined) < 0.15, np.mean(refined) + assert max(refined) < 0.3, max(refined) + + +def test_windowing_does_not_pin_the_fold_to_zero(): + """The failure this consolidation exists to prevent, pinned as a regression. + + A hand-rolled fold once read 0 deg on a rotated map because the rectangular + aperture's axis-aligned leakage dominated. This implementation does not do + that with *or* without the taper -- ``_angular_energy`` drops the DC + neighbourhood and works on magnitude, not power -- and this test is what + would notice if that ever stopped being true. + """ + for deg in (17.0, 31.0): + truth = (-deg) % 90.0 + for window in (False, True): + r = wall_rotation(_room_outline(deg), window=window) + err = abs(r["angle_deg"] - truth) % 90.0 + assert min(err, 90.0 - err) < 1.5, (deg, window, r) + assert r["windowed"] is window + + +def test_fold_90_and_refine_peak_are_usable_standalone(): + """The alignment step imports the pieces, not just the wrapper.""" + angles = np.arange(360) * 0.5 + 0.25 + values = np.zeros(360) + values[40] = values[40 + 180] = 1.0 # one frame: two families 90 deg apart + fa, fv = fold_90(angles, values) + assert len(fa) == 180 + assert fv[40] == 2.0 + values[39], values[41] = 0.5, 0.9 + fa, fv = fold_90(angles, values) + refined = refine_peak(fa, fv, int(np.argmax(fv))) + assert fa[40] < refined < fa[41], (fa[40], refined, fa[41]) + + +def test_wall_rotation_under_reports_below_about_two_degrees(): + """The floor, pinned so a consumer cannot mistake it for a shear measurement. + + A wall line rotated less than ~2 deg rasterises into runs long enough that + the dominant spectral content is still axis-aligned, so the peak is pulled + onto the axis and the rotation is *under-reported* — measured at roughly a + third of truth. An alignment step that corrected a 1-2 deg shear from this + number would silently under-correct, so the shortfall is asserted rather + than described. + """ + + def signed(deg): + v = wall_rotation(_room_outline(deg))["angle_deg"] + return (v + 45.0) % 90.0 - 45.0 + + # Below the floor: badly short of truth. + for deg in (0.5, 1.0, 1.5): + assert abs(signed(deg)) < 0.6 * deg, (deg, signed(deg)) + # At and above it: faithful. + for deg in (2.0, 3.0, 5.0): + assert abs(abs(signed(deg)) - deg) < 0.4, (deg, signed(deg)) + + +def test_wall_rotation_is_invariant_to_incidental_map_extent(): + """Padding the canvas must not move the measured rotation.""" + tight = _room_outline(17.0) + big = np.zeros((700, 820), bool) + big[100 : 100 + tight.shape[0], 220 : 220 + tight.shape[1]] = tight + a, b = wall_rotation(tight), wall_rotation(big) + assert abs(a["angle_deg"] - b["angle_deg"]) < 1e-6, (a, b) + + +def test_tapering_a_tight_crop_would_be_worse_than_not_tapering(): + """Why wall_rotation pads before it tapers. + + Pinned because it is counter-intuitive: adding a window to a tight crop + makes the measurement *worse* than leaving it off, so a future simplification + that drops the padding would quietly halve the accuracy. + """ + from mote_bringup.map_cleanup import angular_stats as mod + + angles = (3.0, 7.25, 12.0, 23.5, 31.0, 44.0) + + def mean_err(pad): + saved, mod.ROTATION_PAD_FRAC = mod.ROTATION_PAD_FRAC, pad + try: + errs = [] + for d in angles: + truth = (-d) % 90.0 + e = abs(wall_rotation(_room_outline(d))["angle_deg"] - truth) % 90.0 + errs.append(min(e, 90.0 - e)) + return float(np.mean(errs)) + finally: + mod.ROTATION_PAD_FRAC = saved + + assert mean_err(0.0) > 2 * mean_err(mod.ROTATION_PAD_FRAC), ( + mean_err(0.0), + mean_err(mod.ROTATION_PAD_FRAC), + ) + + +def test_wall_rotation_degrades_on_a_structureless_mask(): + r = wall_rotation(np.zeros((40, 40), bool)) + assert r["angle_deg"] is None and "note" in r + + +# -------------------------------------------------------------------------- +# Two defects found in review, each pinned by the case that exposed it + + +def _elongated(rot=0.0, n=900, h=170, w=620): + """A perfectly rectilinear building whose bbox stays oblong when rotated. + + A rotated *square-ish* building has a square-ish bounding box, which is why + the ordinary fixtures never exposed the aspect-ratio bug. + """ + m = np.zeros((n, n), bool) + ys = np.linspace(n / 2 - h / 2, n / 2 + h / 2, 3) + xs = np.linspace(n / 2 - w / 2, n / 2 + w / 2, 8) + for y in ys: + m[int(y), int(xs[0]) : int(xs[-1])] = True + for x in xs: + m[int(ys[0]) : int(ys[-1]), int(x)] = True + if not rot: + return m + t = np.radians(rot) + yy, xx = np.mgrid[0:n, 0:n] + sy, sx = yy - n / 2.0, xx - n / 2.0 + iy = np.rint(np.cos(t) * sy - np.sin(t) * sx + n / 2.0).astype(int) + ix = np.rint(np.sin(t) * sy + np.cos(t) * sx + n / 2.0).astype(int) + ok = (iy >= 0) & (iy < n) & (ix >= 0) & (ix < n) + out = np.zeros((n, n), bool) + out[ok] = m[iy[ok], ix[ok]] + return out + + +@pytest.mark.parametrize("rot", [0.0, 10.0, 20.0, 30.0, 40.0]) +def test_an_oblong_intact_building_is_not_reported_as_torn(rot): + """The transform must run on a square canvas or every angle is skewed. + + A frequency-domain index maps to a real frequency divided by that axis' + length, so on an oblong array a genuinely perpendicular pair of wall + families stops looking perpendicular and the frame table invents a tear -- + on precisely the signal this module exists to report. Measured before the + fix: this fixture reported 2 frames at 10, 20 and 30 deg. + + Note which maps the bug spared: 0 and 90 deg are fixed points of the + distortion, so it is invisible on an axis-aligned map and appears only once + the map frame is rotated -- which a real SLAM frame always is. + """ + s = angular_stats(_elongated(rot)) + assert s["n_frames"] == 1, (rot, s["frames"], s["directions"]) + + +def test_transform_canvas_is_square(): + from mote_bringup.map_cleanup.angular_stats import _crop_to_content + + out = _crop_to_content(_elongated(20.0)) + assert out.shape[0] == out.shape[1], out.shape + + +def test_reported_directions_are_wall_orientations_not_normals(): + """A wall's energy lands perpendicular to it; the reported angle must not. + + The spectrum peak for a horizontal wall sits at 90 deg (a horizontal line + transforms to a vertical frequency ridge). A table headed "wall directions" + has to report 0 deg for that wall, so the conversion happens at the + reporting boundary. + """ + horizontal = np.zeros((400, 400), bool) + for y in (80, 160, 240, 320): + horizontal[y, 60:340] = True + vertical = np.zeros((400, 400), bool) + for x in (80, 160, 240, 320): + vertical[60:340, x] = True + + (h,) = _dirs(angular_stats(horizontal)) + (v,) = _dirs(angular_stats(vertical)) + assert min(h, 180.0 - h) < 5.0, h + assert abs(v - 90.0) < 5.0, v + + +def test_wall_and_spectral_angles_round_trip(): + for a in (0.0, 12.5, 89.9, 90.0, 179.5): + assert abs(spectral_to_wall(wall_to_spectral(a)) - a) < 1e-9 + assert 0.0 <= wall_to_spectral(a) < 180.0 + + +def test_a_declared_reference_set_uses_real_wall_angles(): + """`reference_directions` is documented as wall directions, so an exact + declaration of the fixture's true walls must fit with ~no rotation offset.""" + s = angular_stats(pure(), reference_directions=[0.0, 90.0]) + assert s["off_reference_energy_frac"] < 0.10, s + off = abs(s["reference_offset_deg"]) % 90.0 + assert min(off, 90.0 - off) < 2.0, s + + +# -------------------------------------------------------------------------- +# Contract and edges + + +def test_angles_reported_are_pixel_frame_and_bounded(): + s = angular_stats(pure(23.0)) + for d in s["directions"]: + assert 0.0 <= d["angle_deg"] < 180.0 + assert d["width_deg"] >= 0.0 + for f in s["frames"]: + assert 0.0 <= f["angle_deg"] < 90.0 + assert 0.0 <= f["offset_from_dominant_deg"] <= 45.0 + assert abs(sum(d["energy_frac"] for d in s["directions"]) - 1.0) < 1.0 + + +def test_score_is_invariant_to_incidental_map_extent(): + """Padding the grid with never-observed space must not change the score.""" + m = pure() + padded = np.zeros((N + 260, N + 190), bool) + padded[130 : 130 + N, 90 : 90 + N] = m + a, b = angular_stats(m), angular_stats(padded) + assert abs(a["angular_support_deg"] - b["angular_support_deg"]) < 1e-6 + assert abs(a["unassigned_energy_frac"] - b["unassigned_energy_frac"]) < 1e-6 + + +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 + + s = angular_stats(pure(), Params()) + assert s["n_peaks"] == 2 + + +def test_empty_and_degenerate_masks_do_not_raise(): + for m in ( + np.zeros((40, 40), bool), + np.zeros((0, 0), bool), + np.ones((2, 2), bool), + ): + s = angular_stats(m) + assert s["n_peaks"] == 0 + assert "note" in s + + +def test_defaults_match_the_declutter_params(): + """SpectrumParams carries its own copy of the shared defaults; they must agree.""" + from mote_bringup.map_cleanup.structure_extraction import Params + + full, spec = Params(), SpectrumParams() + for f in vars(spec): + assert getattr(full, f) == getattr(spec, f), f + + +if __name__ == "__main__": + sys.exit(pytest.main([__file__, "-v"])) diff --git a/mote_simulation/tools/bag_replay/README.md b/mote_simulation/tools/bag_replay/README.md index 084c399..b9ef98d 100644 --- a/mote_simulation/tools/bag_replay/README.md +++ b/mote_simulation/tools/bag_replay/README.md @@ -80,6 +80,41 @@ identical odometry. read thicker — lower is crisper), `speckle_frac` (isolated occupied cells — scan-match noise, lower is cleaner), `unknown/free/occ_frac`, and `explored_area_m2`. +- **angular structure** (`angular_stats`, slam mode) — a **tear detector**, from + the same FFT orientation spectrum the declutter pass uses. Its job is the one + loop drift cannot do: loop drift is only meaningful when the trajectory + *closes*, so a session that exits on its exploration budget produces no drift + number at all, and for those maps this is the only automated tear signal there + is. + + The signal is the **orthogonal-frame table**. A rectilinear building puts all + its walls in one frame. A drift-rotated *section* duplicates that section's + whole frame — both wall directions, ~90° apart — so a second frame carrying + real energy means part of the map is drawn on its own axes. An angled hallway, + by contrast, adds a single *direction* to the existing frame. `directions: 2` + vs `directions: 1` in the frame table is that distinction. + + Reported alongside: a **wall-direction table**, and `angular_support_deg` (the + effective number of degrees of wall direction the map uses) as a descriptive + scalar. Both are **not ranked and not bolded** — see Limitations for why + `angular_support_deg` must not be used to pick a winner. + + Related: `map_cleanup/room_segmentation.py` assumes "Manhattan after rotation" + and does not support a building with wings at 30° to each other. More than one + frame with real energy share is the measurement that tells you that assumption + is being violated. + + The same module exposes `wall_rotation()` — windowed, folded 0/90, sub-bin + interpolated — which is the canonical way to measure *how far* a map's wall + grid is turned. Map alignment should call it rather than re-deriving the fold. + It is accurate to ~0.07° from 2° up and **under-reports below that** (a true + 1.5° reads ~0.5°), because a barely-rotated line rasterises into runs that are + still spectrally axis-aligned — so it cannot measure a 1–2° residual shear. + + Angles are reported as **wall orientations**. A wall's Fourier energy lies + perpendicular to it, so the raw spectrum peak is the wall *normal*; the + conversion happens at the reporting boundary, and the transform runs on a + square canvas because an oblong one skews every angle towards its long axis. ## Limitations @@ -95,6 +130,25 @@ cannot prove versus the sim's ground truth: - **Crispness ≠ correctness.** A confidently *wrong* map — e.g. a mis-closed loop drawn with sharp walls — can score well on wall thickness and speckle. The crispness proxies catch blur, noise, and incompleteness, not global error. +- **Angular structure detects tears; it does not rank quality.** Do not use + `angular_support_deg` to pick a winner: it is confounded by coverage, since a + map that explored less has fewer long walls and reads as tighter. On the + 2026-07-29 run-3 pair the leg that is clearly better by loop drift (0.551 m vs + 8.776 m) scores *worse* on it (42.2 vs 39.3), having covered 59 m² against + 81 m². Read it beside `explored_area_m2`, or not at all. +- **A multi-angle building is not a defect.** A flat with an angled hallway + genuinely has three dominant wall directions and always will. Nothing here + should be tuned until it calls such a building broken. +- **The frame table is blind below ~10°** — the merge tolerance, which must + exceed the shear a genuine frame carries (the run-3 conservative leg's own + frame is internally sheared 7.5°) or honest shear would be reported as a tear. + It is trustworthy for the tears it is relied on for (≥~20°; run 3's real pair + were 25° and 41° apart, and the synthetic band 20–40° is pinned by tests), + and a smaller rotation will show one frame, not two. Catching that needs a + declared direction set for the site, which `angular_stats(..., + reference_directions=...)` accepts and this report does not yet supply. +- **`n_peaks` is threshold-bound** (`peak_rel_threshold`, `peak_nms_deg`) and + capped, so it is reported and never ranked. - **Not bit-exact.** The recorded sensor stream makes the *input* deterministic, but SLAM's solver is not bit-identical run to run; treat small deltas as noise and lean on the map images for anything marginal. diff --git a/mote_simulation/tools/bag_replay/report.py b/mote_simulation/tools/bag_replay/report.py index 754eee1..a18ca20 100644 --- a/mote_simulation/tools/bag_replay/report.py +++ b/mote_simulation/tools/bag_replay/report.py @@ -39,6 +39,15 @@ def _get(d, dotted): ("occupied frac", "map.occ_frac", 4, ""), ("wall thickness (m)", "map.mean_wall_thickness_m", 3, "lower"), ("speckle frac", "map.speckle_frac", 4, "lower"), + # Angular structure is reported per map under "## Maps", not ranked here. + # It is a *tear detector*, not a quality ordering: on the one real pair + # available it prefers the leg with 16x the loop drift, because that leg + # explored more and a larger map uses more wall directions. Ranking is loop + # drift's job (and, where the trajectory does not close, nobody's) -- so + # these are descriptive columns and bolding a winner among them would be a + # claim the numbers do not support. + ("angular support (°)", "map.angular_support_deg", 2, ""), + ("wall frames (≥15% energy)", "map.n_strong_frames", 0, ""), ] @@ -80,11 +89,57 @@ def build_markdown(run) -> str: else: lines.append("- _no map produced_") lines.append("") + lines += _angular_tables(r["metrics"]) lines += _limitations() return "\n".join(lines) +def _angular_tables(m) -> list: + """Per-map wall-direction and frame structure. + + Structure, not score: these describe *this* map's angular makeup and are not + comparable across parameter sets the way the metrics table is, so they live + here rather than in ``ROWS``. A building with an angled hallway genuinely has + an extra direction; a drift-rotated section duplicates a whole orthogonal + frame. That difference is what the frame table is for. + """ + directions = _get(m, "map.directions") + frames = _get(m, "map.frames") + if not directions: + return [] + + lines = [ + "**Wall directions** (wall orientations, image frame, energy-weighted):", + "", + ] + lines.append("| angle (°) | energy frac | width (°) |") + lines.append("| --- | --- | --- |") + for d in directions: + lines.append( + f"| {_fmt(d.get('angle_deg'), 2)} | {_fmt(d.get('energy_frac'), 3)}" + f" | {_fmt(d.get('width_deg'), 2)} |" + ) + lines.append("") + + if frames: + share = _get(m, "map.dominant_frame_share") + lines.append(f"**Orthogonal frames** (dominant share {_fmt(share, 3)}):") + lines.append("") + lines.append( + "| frame (°) | energy frac | directions | offset from dominant (°) |" + ) + lines.append("| --- | --- | --- | --- |") + for f in frames: + lines.append( + f"| {_fmt(f.get('angle_deg'), 2)} | {_fmt(f.get('energy_frac'), 3)}" + f" | {_fmt(f.get('n_directions'), 0)}" + f" | {_fmt(f.get('offset_from_dominant_deg'), 1)} |" + ) + lines.append("") + return lines + + def _mark(v, vals, better, nd): """Bold the best value in a row when a direction is defined.""" s = _fmt(v, nd) @@ -112,6 +167,33 @@ def _limitations() -> list: "- **Map crispness** (wall thickness, speckle, unknown fraction) catches" " blur, noise, and incompleteness. It does **not** catch a confidently" " *wrong* map: a mis-closed loop drawn with sharp walls scores well here.", + "- **Angular structure** is a **tear detector, not a quality ranking**," + " and is deliberately not bolded. It answers the one question loop drift" + " cannot: loop drift is only meaningful when the trajectory *closes*, so" + " a session that exits on its exploration budget gets no drift number at" + " all, and for those maps the frame table below is the only automated" + " tear signal there is. Read it like this:", + "", + " - **`wall frames` > 1 with real energy share means two rectangular" + " systems in one map** — i.e. a section drawn on its own axes. That is" + " what a SLAM tear looks like. Check the per-map frame table for the" + " offset; run 3's two legs were torn by 25° and 41°.", + " - **One extra *direction* is architecture, not damage.** A flat with" + " an angled hallway genuinely has three wall directions. The frame table" + " distinguishes them: a rotated section duplicates a whole frame" + " (`directions: 2`), a hallway adds one (`directions: 1`).", + " - **It is blind below ~10°**, the frame merge tolerance, which has to" + " exceed the shear a genuine frame carries (7.5° measured on a real leg)" + " or honest shear would read as a tear. A small rotation will show one" + " frame. Catching that needs a declared direction set for the site," + " which `angular_stats(..., reference_directions=...)` accepts and this" + " report does not yet supply.", + " - **`angular support` is confounded by coverage** and must not be" + " used to rank: a map that explored less has fewer long walls and reads" + " as tighter. On the 2026-07-29 run-3 pair the leg that is better by" + " loop drift (0.551 m vs 8.776 m) scores *worse* on it (42.2 vs 39.3)," + " having covered 59 m² against 81 m². It is here to be read beside" + " `explored area`, not to pick a winner.", "- No absolute scale/position check is possible without a reference map or" " survey. For metric-accuracy claims, use the sim benchmark's ATE.", "- Replaying the same recorded sensor stream makes the comparison" diff --git a/mote_simulation/tools/benchmark/README.md b/mote_simulation/tools/benchmark/README.md index da824f8..541ab6e 100644 --- a/mote_simulation/tools/benchmark/README.md +++ b/mote_simulation/tools/benchmark/README.md @@ -64,6 +64,19 @@ Per trial, gated on sim `/clock` (invariant to real-time factor): `wait`), plus aborted NavigateToPose goals. - **motion smoothness** — RMS linear/angular jerk from `cmd_vel` and the number of forward/backward direction reversals. +- **map quality** (`map_quality`, when a finished grid is scored) — crispness + and coverage proxies, plus **angular structure** from + `mote_bringup.map_cleanup.angular_stats`: `angular_support_deg` and the + wall-direction / orthogonal-frame tables. Imported lazily, so this module + keeps its numpy-only, ROS-free contract and still scores where `mote_bringup` + is not on the path — the angular keys are simply absent then. + + Angular structure is a **tear detector, not a quality ranking**, and the sim + does not rank on it: with Gazebo's true pose the sim reports ATE, which is a + strictly better answer. It earns its place in the **bag-replay** harness, + where there is no ground truth — and specifically where the trajectory does + not close, so loop drift is unavailable too. See `../bag_replay/README.md` for + how to read it and what it cannot decide. ## Ground truth diff --git a/mote_simulation/tools/benchmark/metrics.py b/mote_simulation/tools/benchmark/metrics.py index c947da9..285167c 100644 --- a/mote_simulation/tools/benchmark/metrics.py +++ b/mote_simulation/tools/benchmark/metrics.py @@ -197,11 +197,31 @@ def map_quality( * ``speckle_frac`` — fraction of occupied cells with no occupied neighbour, i.e. isolated specks. Poor odometry/scan-matching sprays these; lower is cleaner. + * ``angular_support_deg`` / ``angular_entropy_norm`` / + ``unassigned_energy_frac``, plus the ``directions`` and ``frames`` tables + — how many distinct wall orientations the map uses and how tightly its + energy sits on them, from + :func:`mote_bringup.map_cleanup.angular_stats.angular_stats`. Lower is + better on all three. These catch the failure the crispness proxies are + blind to: a section of the map drawn at the wrong *angle*, which is crisp + and unspeckled and still wrong. Truth-free caveat: a crisp map is not necessarily a *correct* one — a confidently wrong map (e.g. a mis-closed loop drawn sharply) can still score well here. These proxies catch blur, incompleteness, and noise, not global metric error, which needs a surveyed reference the bag does not carry. + + The angular keys are a *consistency* proxy and inherit that caveat twice + over. A small, confidently-wrong map can score well on them — a map that + explored less has fewer long walls and so uses fewer directions, which reads + as tighter (measured: the better of the two run-3 replay legs by loop drift + scores *worse* on angular support, having explored 59 m² against 81 m²), so + read them beside ``explored_area_m2`` and not alone. And a genuinely + multi-angle building — a flat with an angled hallway — honestly uses more + directions than a rectilinear one; that is architecture, not a defect. + Separating drift from architecture needs a prior, which is what + ``angular_stats``' ``reference_directions`` argument is for; this function + does not supply one. """ g = np.asarray(grid) total = int(g.size) @@ -231,7 +251,7 @@ def map_quality( speckle_frac = float(int(isolated.sum()) / n_occ) decided = total - int(unknown.sum()) - return { + out = { "n_cells": total, "unknown_frac": float(int(unknown.sum()) / total), "free_frac": float(int(free.sum()) / total), @@ -241,6 +261,18 @@ def map_quality( "speckle_frac": speckle_frac, } + # Imported here, not at module scope, so this module keeps its numpy-only + # contract: the sim benchmark must still score in an environment that has no + # mote_bringup on the path. angular_stats is numpy-only precisely so this + # stays cheap when it does resolve. + try: + from mote_bringup.map_cleanup.angular_stats import angular_stats + except ImportError: + return out + if n_occ: + out.update(angular_stats(occ)) + return out + def goal_stats(goals) -> dict: """Success rate and time-to-goal over the scripted goal sequence.""" diff --git a/mote_simulation/tools/benchmark/test_metrics.py b/mote_simulation/tools/benchmark/test_metrics.py index 3bfe9bd..b4a77f3 100755 --- a/mote_simulation/tools/benchmark/test_metrics.py +++ b/mote_simulation/tools/benchmark/test_metrics.py @@ -121,6 +121,70 @@ def test_map_quality_speckle_and_unknown(): assert q["explored_area_m2"] > 0.0 +def test_map_quality_angular_keys_and_graceful_degradation(): + """The angular keys ride on an optional import and must not be load-bearing. + + ``metrics`` keeps a numpy-only, ROS-free contract, so a benchmark run in an + environment without ``mote_bringup`` on the path has to still score — just + without the angular half. + """ + grid = np.full((160, 160), 0, dtype=int) + for y0, y1, x0, x1 in ((20, 70, 20, 80), (90, 140, 30, 120)): + grid[y0:y1, x0] = 100 + grid[y0:y1, x1] = 100 + grid[y0, x0:x1] = 100 + grid[y1, x0:x1] = 100 + + ANGULAR = ( + "angular_support_deg", + "angular_entropy_norm", + "unassigned_energy_frac", + "directions", + "frames", + ) + + q = metrics.map_quality(grid, 0.05) + try: + import mote_bringup.map_cleanup.angular_stats # noqa: F401 + except ImportError: + available = False + else: + available = True + + if available: + for k in ANGULAR: + assert k in q, k + # Axis-aligned rectangles: two wall families in one orthogonal frame. + assert q["n_peaks"] == 2, q["directions"] + assert len(q["frames"]) == 1 + assert q["angular_support_deg"] < 25.0 + else: + for k in ANGULAR: + assert k not in q, k + # Either way the crispness half is intact. + assert q["mean_wall_thickness_m"] > 0.0 + + # And with the import forced to fail, the rest of the scoring survives. + import builtins + + real_import = builtins.__import__ + + def _no_mote_bringup(name, *a, **kw): + if name.startswith("mote_bringup"): + raise ImportError("simulated: mote_bringup not on the path") + return real_import(name, *a, **kw) + + builtins.__import__ = _no_mote_bringup + try: + degraded = metrics.map_quality(grid, 0.05) + finally: + builtins.__import__ = real_import + for k in ANGULAR: + assert k not in degraded, k + assert degraded["mean_wall_thickness_m"] == q["mean_wall_thickness_m"] + assert degraded["explored_area_m2"] == q["explored_area_m2"] + + def test_summarize_and_aggregate(): truth, est = _synthetic_run() series = {