Bag-replay scoring: validate parameter choices against real recordings

.gitignore

@@ -15,3 +15,6 @@ scratchpad*
 
 # Sim benchmark outputs (timestamped metrics/reports)
 benchmark_results/
+
+# Bag-replay scoring outputs (timestamped metrics/reports/maps)
+bag_replay_results/

mote_simulation/tools/bag_replay/README.md

@@ -0,0 +1,114 @@
+# Bag-replay scoring
+
+Offline harness that replays a **real recorded mapping bag** through SLAM (or
+kinematic_icp) under two or more parameter sets and scores the results with
+**truth-free** proxies, then writes a side-by-side comparison report (metrics
+table + rendered maps).
+
+This is the reality check that complements the sim benchmark
+(`mote_simulation/tools/benchmark/`, `pixi run bench`). The sim proves
+parameters against *simulated* sensors and can report absolute error because
+Gazebo publishes the robot's true pose. A real bag carries no ground truth, so
+this harness scores *self-consistency* and *map appearance* instead — see
+[Limitations](#limitations). The two share one metrics module: the truth-free
+functions used here (`map_quality`, `loop_drift`, `path_length`) live in the
+benchmark's `metrics.py`, reused not forked.
+
+## Usage
+
+```bash
+pixi run bag-replay -- \
+  --bag ~/.mote/bags/mapping/<timestamp> \
+  --params mote_bringup/config/slam_toolbox_params.yaml \
+           mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml
+```
+
+- `--bag` — a recorded **mapping** stream bag directory (`/tf`, `/tf_static`,
+  `/scan_filtered`). These are recorded by default on every mapping session
+  (`mapping_launch.py` → `record_launch.py streams:=mapping`) and stamped into
+  the map revision by `save-map`; `pixi run site info` shows which bag built a
+  map. Bags live under the robot's `~/.mote/bags/mapping/` — copy one over to
+  score it (no code is synced *to* the Pi).
+- `--params a.yaml b.yaml …` — one **complete** `slam_toolbox` params file per
+  set (slam mode). Default: the committed `slam_toolbox_params.yaml` as a single
+  baseline. Compare it against `examples/sparse_no_loop.yaml` (a variant with
+  aggressive scan decimation + loop closure off) for a ready two-set example.
+- `--mode slam|icp` — `slam` (default) scores the map + map-frame trajectory;
+  `icp` scores kinematic_icp's odometry self-consistency (no map). ICP params
+  are inline in the launch, so in icp mode `--params` are just repeat labels.
+- `--rate` — replay speed × realtime (default 1.0). 2–3× is safe on a
+  workstation; too fast and SLAM can drop scans in wall time.
+- `--max-scans N` — stop after N scans (debug/quick smoke).
+
+Output lands in `bag_replay_results/<UTC>/`: `report.md` (open it — the map
+images are relative links), `run.json` (full metrics + provenance), and per set
+a `stack.log`, `replay.log`, `series.json` (re-scorable trajectory), `map.npz`
+(occupancy grid) and `map.png`.
+
+## How it works
+
+`replay.py` (orchestrator) runs each parameter set in its **own freshly-launched
+stack under a random `ROS_DOMAIN_ID`** — so a stray node can never reach a live
+robot, and sequential runs can't leak into each other (the sim sweep learned
+that DDS-isolation lesson the hard way). Per set it:
+
+1. launches `replay_stack_launch.py` (env packages only — `slam_toolbox` /
+   `kinematic_icp` / `nav2_lifecycle_manager`, no workspace build needed),
+2. runs `replayer.py`, which streams the bag's sensor + TF messages back onto
+   the graph in **sim-time order** (it drives `/clock` from the bag stamps, so
+   the stack runs on bag time regardless of wall speed), records the estimator's
+   output trajectory, and grabs the finished `/map`,
+3. scores with the shared `metrics` module and renders the map (`render.py`),
+4. tears the stack down and moves on. `report.py` then builds the comparison.
+
+**TF ownership** is the one subtlety. A mapping bag's `/tf` already contains the
+edges the *original* run produced — `map→odom` (slam_toolbox) and
+`odom→base_footprint` (kinematic_icp). Replaying those verbatim would fight the
+fresh node publishing the same edge, so the edge the node-under-test owns is
+stripped from the replayed `/tf`; everything else passes through. In slam mode
+the recorded `odom→base` is kept and *fed* to slam as its odometry prior,
+exactly as on the robot — so a slam-parameter comparison replays against
+identical odometry.
+
+## Metrics (all truth-free)
+
+- **loop drift** (`loop_drift`) — start↔end distance of the estimated
+  trajectory and its ratio to path length. Small on a bag where the robot
+  physically returned to its start ⇒ little accumulated drift.
+- **map crispness / coverage** (`map_quality`, slam mode) — from the finished
+  occupancy grid: `mean_wall_thickness_m` (iterated erosion; blur/double-walls
+  read thicker — lower is crisper), `speckle_frac` (isolated occupied cells —
+  scan-match noise, lower is cleaner), `unknown/free/occ_frac`, and
+  `explored_area_m2`.
+
+## Limitations
+
+These are **proxies, not error measures** — be honest about what they can and
+cannot prove versus the sim's ground truth:
+
+- **No absolute accuracy.** Without a surveyed reference the bag cannot tell you
+  the map is metrically *correct*, only whether it is self-consistent and looks
+  clean. For metric-accuracy claims (ATE), use the sim benchmark.
+- **Loop drift needs a loop.** It cannot distinguish a legitimate open A→B
+  traverse (honestly large end distance) from a drifting loop. Know the bag's
+  shape before reading it.
+- **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.
+- **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.
+
+## Tests
+
+`test_bag_replay.py` and the benchmark's `test_metrics.py` cover the ROS-free
+pieces (metrics, scoring, rendering, report, domain-ID isolation) with no ROS
+graph, so the test path can never touch a live robot. Run them with:
+
+```bash
+python mote_simulation/tools/bag_replay/test_bag_replay.py
+python mote_simulation/tools/benchmark/test_metrics.py
+```
+
+The ROS replay itself needs `slam_toolbox` and a real bag and is exercised by
+`pixi run bag-replay`, not by the unit tests.

mote_simulation/tools/bag_replay/examples/sparse_no_loop.yaml

@@ -0,0 +1,73 @@
+slam_toolbox:
+  ros__parameters:
+
+    # Frames
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_link
+    scan_topic: /scan_filtered
+    mode: mapping
+
+    # Solver
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # Map
+    resolution: 0.05
+    map_update_interval: 5.0
+
+    # Laser — RPLIDAR C1: 12 m range, 10 Hz
+    max_laser_range: 16.0
+    minimum_time_interval: 0.5
+
+    # TF
+    transform_publish_period: 0.02
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+
+    # Scan matching — aggressive decimation: integrate far fewer scans.
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.8
+    minimum_travel_heading: 0.8
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+
+    # Loop closure — disabled, so accumulated drift is never corrected.
+    do_loop_closing: false
+    loop_search_maximum_distance: 3.0
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    # Loop search space
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Penalties
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: false
+
+    debug_logging: false
+    throttle_scans: 1
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true

mote_simulation/tools/bag_replay/render.py

@@ -0,0 +1,55 @@
+"""Render a replayed occupancy grid to a PNG, optionally with the trajectory.
+
+Uses cv2 (already a project dependency — map_cleanup and sites.py save maps with
+it), matching map_saver's convention: occupied black, free white, unknown grey,
+north up (grid row 0 is the bottom, so the image is flipped vertically). The
+estimator trajectory, if given, is drawn in the map frame over the grid.
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+OCC_THRESH = 65
+FREE_THRESH = 25
+
+
+def grid_to_bgr(grid: np.ndarray) -> np.ndarray:
+    """int8 occupancy grid (0..100, -1 unknown) -> flipped BGR image."""
+    img = np.full(grid.shape, 205, dtype=np.uint8)  # unknown grey (map_saver value)
+    img[(grid >= 0) & (grid <= FREE_THRESH)] = 254  # free
+    img[grid >= OCC_THRESH] = 0  # occupied
+    img = np.flipud(img)  # ROS grid origin is bottom-left; image north is up
+    return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+
+
+def render_map(npz_path, out_png, traj=None) -> None:
+    data = np.load(npz_path)
+    grid = data["grid"]
+    res = float(data["resolution"])
+    ox, oy = (float(v) for v in data["origin"])
+    h = grid.shape[0]
+    img = grid_to_bgr(grid)
+
+    if traj:
+        pts = []
+        for row in traj:
+            _, x, y = row[0], row[1], row[2]
+            px = int(round((x - ox) / res))
+            py = int(round((y - oy) / res))
+            py = h - 1 - py  # match the vertical flip
+            pts.append([px, py])
+        if len(pts) >= 2:
+            cv2.polylines(img, [np.array(pts, dtype=np.int32)], False, (0, 128, 255), 1)
+            cv2.circle(img, tuple(pts[0]), 3, (0, 200, 0), -1)  # start green
+            cv2.circle(img, tuple(pts[-1]), 3, (0, 0, 255), -1)  # end red
+
+    # Upscale small maps so features are legible in the report.
+    scale = max(1, int(round(900 / max(img.shape[:2]))))
+    if scale > 1:
+        img = cv2.resize(img, None, fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST)
+    Path(out_png).parent.mkdir(parents=True, exist_ok=True)
+    cv2.imwrite(str(out_png), img)