diff --git a/dimos/mapping/relocalization/README.md b/dimos/mapping/relocalization/README.md new file mode 100644 index 0000000000..03006c690c --- /dev/null +++ b/dimos/mapping/relocalization/README.md @@ -0,0 +1,195 @@ +# Relocalization: mode-switched tracking + +Adds a cheap "tracking" mode to `RelocalizationModule`, alongside the existing +full FPFH+RANSAC+ICP global search, plus a ground-truth benchmark harness to +validate the trade-off instead of only trusting self-reported ICP fitness. + +## How to run it + +Everything below assumes `uv` is installed and you're in the repo root +(`uv run` auto-syncs dependencies on first use — no manual `pip install` +needed). First run downloads `go2_hongkong_office.db` and its premap from +Git LFS automatically. + +**One-shot comparison** (recommended — runs both modes, prints a table, saves plots): + +```bash +./dimos/mapping/relocalization/compare.sh go2_hongkong_office go2_hongkong_office_twopass_map 15 +``` + +Writes logs and 4 PNGs (per-attempt latency, per-attempt translation error, +total-wall-time bar chart, fitness-vs-real-error scatter) to a fresh +`/tmp/reloc_comparison_/`. + +**Either mode individually:** + +```bash +PYTHONUNBUFFERED=1 uv run python -m dimos.mapping.relocalization.eval go2_hongkong_office \ + --map-file go2_hongkong_office_twopass_map --mode global --max-attempts 15 # baseline + +PYTHONUNBUFFERED=1 uv run python -m dimos.mapping.relocalization.eval go2_hongkong_office \ + --map-file go2_hongkong_office_twopass_map --mode tracking --max-attempts 15 # our method +``` + +(`PYTHONUNBUFFERED=1` matters if you're piping/teeing output — Python fully +buffers stdout when it isn't a TTY, so without it you won't see anything +until the process exits.) + +`--max-attempts` caps the run — each full search costs 15-70s depending on +machine load, so a 15-attempt run is a reasonable smoke test; drop it to +benchmark the whole recording. + +To re-plot from existing logs without re-running the (slow) benchmark: + +```bash +uv run python -m dimos.mapping.relocalization.plot_comparison \ + --global-log --tracking-log --out-dir +``` + +## The gap this fills + +Went looking for gaps in the navigation/relocalization stack before landing +here (see `docs/capabilities/navigation/relocalization.md` for how the +existing system works). Several candidates came up: no ground-truth eval for +relocalization accuracy (only self-reported ICP fitness), no pose-jump/drift +detection on the published TF, no tracking/warm-start mode (every cycle pays +the full global-search cost), no multi-premap support, no last-known-pose +persisted to non-volatile storage across power cycles to seed the next +session's relocalization search instead of starting cold, no richer +localization-quality signal exposed downstream. Picked the tracking/warm-start +gap specifically, for reasons below. + +## Why this feature, and why not the alternatives + +**Scoped for a personal laptop, no GPU dependency, and a tight time budget.** +`relocalize()`'s actual registration work (FPFH, RANSAC, ICP) runs through +Open3D's legacy **CPU** pipeline, not the CUDA tensor API — only the +`VoxelGrid` map accumulation uses GPU, and that's not the bottleneck. So this +feature needed no GPU passthrough, no simulation environment, no live robot — +just a recorded dataset already sitting in Git LFS, replayed offline. That +made it tractable to build and validate end-to-end in the time available. + +**Purposefully not pursuing a live factor-graph approach.** The existing +system already uses factor graphs (GTSAM) for premap construction — see +`dimos/mapping/loop_closure/pgo.py` — but strictly offline: it needs to see +the *whole* recording, including future loop closures, to correct past poses. +Building an online, live factor-graph-based correction system (continuously +re-optimizing a pose graph against streaming relocalization observations) +would be a substantially larger undertaking — new state management, new +failure modes, much more surface area to validate — and didn't fit a +few-hour scope. The mode-switch approach reuses the existing, well-tuned +global search unchanged and adds a much smaller, bounded piece of new logic +on top. + +**Purposefully not pursuing a LIO-SAM-style integration.** This codebase +already has FAST-LIO2 and Point-LIO (`dimos/hardware/sensors/lidar/`) for +platforms with a real 3D LiDAR (mid360 rigs, the G1 humanoid) — LIO-SAM would +mean bolting loop closure onto one of those existing LIO front-ends. That's +real work, but it's *integrating an existing, well-documented open-source +system* rather than a feature we could reason about and validate ourselves in +this scope — and it doesn't even apply to the dataset used here (this +recording comes from the standard WebRTC-based Go2 pipeline, not a +mid360/FAST-LIO one), so it wouldn't have been testable with the data at +hand. + +## What I hoped to achieve + +The measured baseline showed `relocalize()` costing 13-70s wall-clock per +call (machine-load dependent) against a nominal 2-second re-localization +budget (`RELOC_INTERVAL`) — meaning the live system cannot keep +up with its own throttle interval. Goal: cut that cost dramatically for the +common case (already have a recent, good lock) while keeping accuracy in the +same ballpark, by reusing the last good alignment as a seed for a cheap local +ICP instead of re-running the full global search every cycle — falling back +to the global search whenever that seed can no longer be trusted. + +## Architecture + +- **`relocalize.py`**: added `track(global_map, local_map, seed_T, yaw_fan_deg)` + — no FPFH, no RANSAC, a small fan of seeded ICP attempts around `seed_T`, + scored by the same wall-only fitness trick the existing global search + already uses (to avoid floor/ceiling rotational-symmetry masking a + wrong-yaw candidate). `_wall_subset` was moved to module level so both + paths share it. +- **`module.py`**: `RelocalizationModule` now tracks `last_good = (T, fitness, + ts)`, a missed counter, and the position/time of the last full search. + `should_force_full_search()` decides SEARCHING vs. TRACKING each cycle + (no lock yet / stale lock / too many misses / too long or too far since an + absolute reacquisition). TRACKING seeds `track()` from `last_good.T` + directly. SEARCHING calls the original `relocalize()`, unchanged. +- **`eval.py`**: offline ground-truth benchmark. Replays a recording's own + `lidar` stream through the same accumulation logic `VoxelGridMapper` uses + live, calls `relocalize()`/`track()` at the same cadence, and compares the + output against the *recording's own* PGO-corrected trajectory — + `PoseGraph.correction_at(ts)` which is the transform relocalization is + trying to estimate, so replaying a recording against a premap built from + itself gives a ground truth substitue with no extra data collection. +- **`compare.sh` / `plot_comparison.py`**: run both modes back-to-back and + turn the logs into a comparison table + plots. + +## Observations + +- **~48-67% reduction in total wall-clock time** across a 15-attempt run + (exact number depends on how often the reset get triggered — see tuning + below), with per-tracking-attempt latency dropping from 13-20s to ~0.05-0.1s. +- **Tracking-mode accuracy lands within a modest, bounded margin of the + baseline** — typically 20-80mm worse median translation error, + under a degree worse median yaw error. +- **Two regressions found and fixed during development, both worth knowing + about**: (1) briefly tried seeding the tracking-mode search on the robot's + own odometry-measured heading change, which caused yaw errors up to ~100° + — a real robot turn is already correctly represented in the accumulated + map via per-frame odometry placement, so re-centering the search on "how + much did the robot turn" double-counts real motion as if it were error. + (2) A subsequent fix (widening the search fan based on distance/time since + the last successful attempt) turned out to silently reset every ~2 seconds + because `last_good` updates on every tracking success too — so the fan + almost never actually widened across a streak. Fixed by keying the + fan-width calculation off the last *full search* instead, which only + resets on an absolute reacquisition. +- **The residual accuracy gap looks structural, not something a + reset-cadence knob can fix.** Even the untouched global search's own + accuracy trended worse (~0.34m → ~0.42m over a 30-second + replay window, no tracking mode involved at all) — pointing at the + accumulated map's own shape evolving as column-carving keeps replacing + older, less-drifted regions with newer, more odometry-drift-affected ones. +- **Self-reported ICP fitness does not reliably reveal this kind of drift.** + Within a tracking streak, fitness *increased* (0.92→0.96) while real + ground-truth error also increased (0.37m→0.45m) — ICP reports how well it + converged to something nearby, not whether that something is still + correct. `fitness_vs_trans_err.png` from `compare.sh` shows this visually. + +## Known limitations / untested edges + +- `MAX_TRACKING_MISSES` (3) has never actually fired in any eval run — every + tracking attempt across every run so far has been accepted, so this + boundary condition is unvalidated. +- `SANITY_CHECK_INTERVAL` (30s) exists for the "robot barely moving, or + picked up while stationary" case — the test recording keeps moving + continuously, so the distance trigger always fires first and this constant + has never actually been exercised. Kept close to `MAX_TRACKING_AGE` for + internal consistency, not validated against data. +- The yaw fan is yaw-only — there's no equivalent perturbation for translation. +- Ground truth requires the premap to be built from the *same* recording + being replayed (for the `correction_at(ts)` trick to hold) — this eval + harness can't currently validate against an independently-collected premap. + +## Scope of improvement + +- **A translation-perturbation fan**, mirroring the yaw fan, sized similarly + by distance/time since the last full search. +- **A heatmap of where fitness/error consistently degrades**, using the + per-attempt logs already being produced, as a tool to flag premap regions + needing a re-scan. +- **Classify LiDAR points as static vs. dynamic** before the ICP correspondence + search, so transient obstacles that never appear in the static premap don't + get treated as failed or false correspondences in scenes with heavier + traffic than the built map. +- **Tune the drift-rate assumption per robot type, instead of one fixed + number for every platform** The current constant (0.3°/m) was only + tested on the Go2 — a quadruped, a humanoid, and a drone all drift + differently, so this number likely won't transfer as-is. Not a quick + fix, though: it needs real uncertainty data from IMU/odometry, and most + robot drivers in this codebase don't currently produce that. The actual + blocker is missing sensor infrastructure elsewhere, not something + fixable inside this feature alone. diff --git a/dimos/mapping/relocalization/compare.sh b/dimos/mapping/relocalization/compare.sh new file mode 100755 index 0000000000..a7d7ff8d87 --- /dev/null +++ b/dimos/mapping/relocalization/compare.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Runs the global (baseline, every attempt = full FPFH+RANSAC+ICP search) and +# tracking (our method, mode-switched) relocalization benchmarks back-to-back +# against the same recording, then builds a comparison table + plots. +# +# Usage: +# ./compare.sh [recording] [map_file] [max_attempts] +# ./compare.sh go2_hongkong_office go2_hongkong_office_twopass_map 15 + +set -euo pipefail + +RECORDING="${1:-go2_hongkong_office}" +MAP_FILE="${2:-go2_hongkong_office_twopass_map}" +MAX_ATTEMPTS="${3:-15}" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "$SCRIPT_DIR/../../.." && pwd)" +OUT_DIR="/tmp/reloc_comparison_$(date +%s)" +mkdir -p "$OUT_DIR" + +GLOBAL_LOG="$OUT_DIR/global.log" +TRACKING_LOG="$OUT_DIR/tracking.log" + +cd "$REPO_ROOT" +export PATH="$HOME/.local/bin:$PATH" + +echo "=== [1/3] running GLOBAL (baseline, every attempt = full search) ===" +PYTHONUNBUFFERED=1 uv run python -m dimos.mapping.relocalization.eval "$RECORDING" \ + --map-file "$MAP_FILE" --mode global --max-attempts "$MAX_ATTEMPTS" \ + 2>&1 | tee "$GLOBAL_LOG" + +echo +echo "=== [2/3] running TRACKING (our method) ===" +PYTHONUNBUFFERED=1 uv run python -m dimos.mapping.relocalization.eval "$RECORDING" \ + --map-file "$MAP_FILE" --mode tracking --max-attempts "$MAX_ATTEMPTS" \ + 2>&1 | tee "$TRACKING_LOG" + +echo +echo "=== [3/3] building comparison table + plots ===" +uv run python -m dimos.mapping.relocalization.plot_comparison \ + --global-log "$GLOBAL_LOG" --tracking-log "$TRACKING_LOG" --out-dir "$OUT_DIR" + +echo +echo "logs: $GLOBAL_LOG" +echo " $TRACKING_LOG" +echo "plots: $OUT_DIR/*.png" diff --git a/dimos/mapping/relocalization/eval.py b/dimos/mapping/relocalization/eval.py new file mode 100644 index 0000000000..4eca5d53e9 --- /dev/null +++ b/dimos/mapping/relocalization/eval.py @@ -0,0 +1,346 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Ground-truth accuracy + latency benchmark for live relocalization. + +`RelocalizationModule` only ever sees `fitness` (ICP's own self-reported +score) — as of now, that score is not verified against real pose error. This +script replays a recording's own `lidar` stream through the same +accumulation `VoxelGridMapper` uses (`VoxelGrid`, column-carved), calls the +production `relocalize()`/`track()` at the same cadence the live module +does, and compares the output against the recording's own PGO-corrected +trajectory. + +The ground-truth trick: build the premap from a recording via +`dimos map global {name} --export`, and that premap sits in the PGO +`world_corrected` frame. `PoseGraph.correction_at(ts)` (from +`dimos/mapping/loop_closure/pgo.py`) returns exactly the +`world_corrected <- world_raw` transform at a given timestamp — which is +the same quantity `relocalize()` is trying to estimate live (aligning a +live raw-odometry submap onto the premap). So replaying a recording +against a premap built from *itself* gives a reference ground truth. + +Usage: + # Baseline: every attempt uses the full FPFH+RANSAC+ICP search (default). + uv run python -m dimos.mapping.relocalization.eval go2_hongkong_office \ + --map-file go2_hongkong_office_twopass_map + + # Quick smoke run before committing to a full pass (each relocalize() + # call can take several seconds — see relocalization.md's sample log). + uv run python -m dimos.mapping.relocalization.eval go2_hongkong_office \ + --map-file go2_hongkong_office_twopass_map --max-attempts 5 + + # Tracking mode: exercises the live SEARCHING/TRACKING gate + # (RelocalizationModule.should_force_full_search / yaw_fan_for_travel), + # reporting search vs. tracking attempts separately. + uv run python -m dimos.mapping.relocalization.eval go2_hongkong_office \ + --map-file go2_hongkong_office_twopass_map --mode tracking + +See `compare.sh` to run both modes and produce a comparison table + plots +in one shot, and `README.md` in this directory for the full design writeup. +""" + +from __future__ import annotations + +import time +from dataclasses import dataclass +from typing import Any + +import numpy as np +import typer + +from dimos.mapping.loop_closure.pgo import PGO +from dimos.mapping.relocalization.module import ( + MAP_SUFFIX, + MIN_LOCAL_POINTS, + RELOC_INTERVAL, + should_force_full_search, + yaw_fan_for_travel, +) +from dimos.mapping.relocalization.relocalize import relocalize, track +from dimos.mapping.voxels import VoxelGrid +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.data import get_data, resolve_named_path + + +@dataclass +class Attempt: + ts: float + n_pts: int + fitness: float + wall_s: float + cpu_s: float + trans_err_m: float + yaw_err_deg: float + accepted: bool + mode: str = "search" # "search" (full relocalize()) or "tracking" (seeded track()) + + +def _pose_error(T_est: np.ndarray, T_gt: object) -> tuple[float, float]: + """Translation (m) and yaw (deg) error between relocalize()'s estimate + and the ground-truth world_raw -> world_corrected transform at this ts.""" + t_est = T_est[:3, 3] + t_gt = T_gt.translation.to_numpy() # type: ignore[attr-defined] + trans_err = float(np.linalg.norm(t_est - t_gt)) + + R_est = T_est[:3, :3] + R_gt = T_gt.rotation.to_rotation_matrix() # type: ignore[attr-defined] + R_diff = R_est.T @ R_gt + yaw_err = float(np.degrees(np.arccos(np.clip((np.trace(R_diff) - 1) / 2, -1.0, 1.0)))) + return trans_err, yaw_err + + +def _odom_at(odom_stream: Any, ts: float) -> Any | None: + """Nearest odom PoseStamped at `ts`, or None if none nearby.""" + try: + obs = odom_stream.at(ts, tolerance=2.0).first() + except Exception: + return None + return obs.data + + +def run_eval( + recording: str, + map_file: str, + *, + sample_interval: float = RELOC_INTERVAL, + fitness_threshold: float = 0.45, + tracking_fitness_threshold: float = 0.5, + voxel_size: float = 0.05, + max_attempts: int | None = None, + mode: str = "global", +) -> list[Attempt]: + """Replay `recording` against `map_file`, returning one `Attempt` per + relocalize()/track() call. `map_file` should be a premap built from + `recording` itself (`dimos map global {recording} --export`) so the + ground-truth trick above holds. + + `mode="global"` (default, the original baseline behavior of this script) + benchmarks the untouched full search on every attempt. `mode="tracking"` + exercises the exact SEARCHING/TRACKING gate the live `RelocalizationModule` + runs, via the same `should_force_full_search`/`yaw_fan_for_travel` + functions it imports its logic from -- see those functions' docstrings + in `module.py` for the fan-sizing design history -- so a single run shows + both the bootstrap full-search cost and steady-state tracking cost. + """ + if mode not in ("global", "tracking"): + raise ValueError(f"mode must be 'global' or 'tracking', got {mode!r}") + + db_path = get_data(f"{recording}.db") + premap_path = resolve_named_path(map_file, MAP_SUFFIX) + premap = PointCloud2.lcm_decode(premap_path.read_bytes()) + + attempts: list[Attempt] = [] + with SqliteStore(path=str(db_path)) as store: + lidar = store.streams.lidar + odom_stream = store.streams.odom + + print("running PGO for ground truth...") + pgo_wall0 = time.monotonic() + graph = lidar.transform(PGO()).last().data + print( + f"PGO ground truth ready in {time.monotonic() - pgo_wall0:.1f}s " + f"({len(graph.keyframes)} keyframes, {len(graph.loops)} loop closures)" + ) + if not graph.keyframes: + raise SystemExit( + f"PGO produced no keyframes for {recording!r} — recording too short/static?" + ) + + # Tracking-mode state -- mirrors RelocalizationModule's instance + # state, kept as local variables since this is a plain loop, not an + # rx pipeline. Unused when mode="global". + last_good: tuple[np.ndarray, float, float] | None = None + miss_count = 0 + last_full_search_ts = 0.0 + last_full_search_pos: np.ndarray | None = None + + grid = VoxelGrid(voxel_size=voxel_size, carve_columns=True, show_startup_log=False) + try: + last_attempt_ts = float("-inf") + for obs in lidar: + grid.add_frame(obs.data) + if obs.ts - last_attempt_ts < sample_interval: + continue + + local = grid.get_global_pointcloud2() + if len(local) < MIN_LOCAL_POINTS: + continue + last_attempt_ts = obs.ts + + latest_odom = _odom_at(odom_stream, obs.ts) + latest_odom_xy = ( + np.array([latest_odom.position.x, latest_odom.position.y]) + if latest_odom is not None + else None + ) + use_tracking = ( + mode == "tracking" + and last_good is not None + and not should_force_full_search( + obs.ts, + last_good, + miss_count, + last_full_search_ts, + last_full_search_pos, + latest_odom_xy, + ) + ) + + cpu0, wall0 = time.process_time(), time.monotonic() + if use_tracking: + assert last_good is not None + seed_T, _, _ = last_good + # Fan seed/width driven by distance/time since the last FULL + # SEARCH, not since last_good -- see module.py's + # yaw_fan_for_travel docstring for why (design history of + # two prior regressions this avoids). + distance_since_full_search = 0.0 + if latest_odom_xy is not None and last_full_search_pos is not None: + distance_since_full_search = float( + np.linalg.norm(latest_odom_xy - last_full_search_pos) + ) + fan = yaw_fan_for_travel( + distance_since_full_search, obs.ts - last_full_search_ts + ) + T, fitness = track( + premap.pointcloud, local.pointcloud, seed_T, yaw_fan_deg=fan + ) + attempt_mode = "tracking" + threshold = tracking_fitness_threshold + else: + # Original baseline path -- unchanged, still what runs on every + # attempt when mode="global" (the default). + T, fitness = relocalize(premap.pointcloud, local.pointcloud) + attempt_mode = "search" + threshold = fitness_threshold + wall_s = time.monotonic() - wall0 + cpu_s = time.process_time() - cpu0 + + accepted = fitness >= threshold + if mode == "tracking": + if accepted: + miss_count = 0 + last_good = (T, fitness, obs.ts) + if attempt_mode == "search": + last_full_search_ts = obs.ts + last_full_search_pos = latest_odom_xy + elif attempt_mode == "tracking": + miss_count += 1 + + trans_err, yaw_err = _pose_error(T, graph.correction_at(obs.ts)) + a = Attempt( + obs.ts, + len(local), + fitness, + wall_s, + cpu_s, + trans_err, + yaw_err, + accepted, + mode=attempt_mode, + ) + attempts.append(a) + print( + f"t={a.ts:8.2f} n_pts={a.n_pts:7d} mode={a.mode:8s} fitness={a.fitness:.3f} " + f"wall={a.wall_s:5.2f}s cpu={a.cpu_s:5.2f}s " + f"trans_err={a.trans_err_m:.3f}m yaw_err={a.yaw_err_deg:6.2f}deg " + f"{'ACCEPT' if a.accepted else 'reject'}" + ) + if max_attempts is not None and len(attempts) >= max_attempts: + break + finally: + grid.dispose() + return attempts + + +def _summarize_group(label: str, group: list[Attempt]) -> None: + if not group: + return + accepted = [a for a in group if a.accepted] + wall = [a.wall_s for a in group] + cpu = [a.cpu_s for a in group] + + print() + print(f"-- {label} (n={len(group)}) --") + print(f"accepted={len(accepted)} ({100 * len(accepted) / len(group):.0f}%)") + print(f"latency wall median={np.median(wall):.2f}s p90={np.percentile(wall, 90):.2f}s") + print(f"latency cpu median={np.median(cpu):.2f}s p90={np.percentile(cpu, 90):.2f}s") + + if accepted: + trans = [a.trans_err_m for a in accepted] + yaw = [a.yaw_err_deg for a in accepted] + print(f"trans_err (accepted) median={np.median(trans):.3f}m p90={np.percentile(trans, 90):.3f}m") + print(f"yaw_err (accepted) median={np.median(yaw):.2f}deg p90={np.percentile(yaw, 90):.2f}deg") + + false_accepts = [a for a in accepted if a.trans_err_m > 0.5] + if false_accepts: + print( + f"WARNING: {len(false_accepts)}/{len(accepted)} accepted attempts have >0.5m " + "ground-truth error — fitness_threshold alone did not catch these" + ) + + +def _summarize(attempts: list[Attempt]) -> None: + if not attempts: + print("no attempts recorded (recording too short, or MIN_LOCAL_POINTS never reached)") + return + + _summarize_group("overall", attempts) + search = [a for a in attempts if a.mode == "search"] + tracking = [a for a in attempts if a.mode == "tracking"] + if search and tracking: + _summarize_group("search (bootstrap / reacquire / sanity-check)", search) + _summarize_group("tracking (seeded, cheap)", tracking) + + +def main( + recording: str = typer.Argument(..., help="Recording stem, e.g. go2_hongkong_office"), + map_file: str = typer.Option( + ..., "--map-file", help="Premap stem, built FROM `recording` itself (see module docstring)" + ), + mode: str = typer.Option( + "global", + "--mode", + help="'global': every attempt uses the full search (original baseline). " + "'tracking': exercises the live SEARCHING/TRACKING gate.", + ), + sample_interval: float = typer.Option( + RELOC_INTERVAL, "--sample-interval", help="Seconds between relocalize() attempts" + ), + fitness_threshold: float = typer.Option(0.45, "--fitness-threshold"), + tracking_fitness_threshold: float = typer.Option( + 0.5, "--tracking-fitness-threshold", help="Only used when --mode=tracking" + ), + voxel_size: float = typer.Option(0.05, "--voxel-size"), + max_attempts: int = typer.Option( + 0, "--max-attempts", help="Stop after N attempts (0 = run the whole recording)" + ), +) -> None: + attempts = run_eval( + recording, + map_file, + sample_interval=sample_interval, + fitness_threshold=fitness_threshold, + tracking_fitness_threshold=tracking_fitness_threshold, + voxel_size=voxel_size, + max_attempts=max_attempts or None, + mode=mode, + ) + _summarize(attempts) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/mapping/relocalization/module.py b/dimos/mapping/relocalization/module.py index 0d9d6f9634..317c94bd17 100644 --- a/dimos/mapping/relocalization/module.py +++ b/dimos/mapping/relocalization/module.py @@ -23,7 +23,9 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.mapping.relocalization.relocalize import relocalize as _relocalize +from dimos.mapping.relocalization.relocalize import track as _track from dimos.mapping.voxels import VoxelGrid +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -42,6 +44,122 @@ MIN_LOCAL_POINTS = 50_000 MAP_SUFFIX = ".pc2.lcm" +# Tracking mode: reuse the last accepted transform as a seed for a cheap +# ICP instead of a full FPFH+RANSAC search (see relocalize.track()). These +# gate how long/how far we trust that seed before forcing a fresh full +# search via `relocalize()`. +MAX_TRACKING_AGE = 30.0 # seconds since last_good before forcing a full search +MAX_TRACKING_MISSES = 3 # consecutive tracking misses before forcing a full search +# Untested edge case: no eval run so far has ever hit this -- every tracking +# attempt across every run has been accepted, so miss_count has never once +# incremented in practice. Worth treating as unvalidated, not proven. +SANITY_CHECK_DISTANCE = 2.0 # meters of straight-line odom displacement since +# the last full search before forcing another one, even if tracking is +# otherwise succeeding -- new map area carved in since then may have shifted +# the best-fit alignment slightly. Tuned empirically across several eval +# runs at 3.0/2.0/1.5/1.0m: tightening below ~2.0 buys back little to no +# accuracy (both global-only and tracking accuracy drift similarly across a +# session -- see README) while giving back most of the compute savings, so +# 2.0 is the settled value, not the tightest one tried. +SANITY_CHECK_INTERVAL = 45.0 # seconds since the last full search before +# forcing another one regardless of distance -- catches a robot picked up +# and moved while stationary, which a distance-only trigger would miss. +# Unlike SANITY_CHECK_DISTANCE, this has never actually been the trigger in +# any eval run (the test recording keeps moving, so distance always fires +# first) -- kept close to MAX_TRACKING_AGE for internal consistency rather +# than validated against data. +YAW_FAN_DEG = (0.0, 5.0, -5.0, 10.0, -10.0) # tried in this order; first hit wins +# ^ kept as the max-width fallback fan (see `yaw_fan_for_travel` below) -- +# used only once distance/time-based sizing alone would already justify a +# wide search. +ASSUMED_YAW_DRIFT_DEG_PER_M = 0.3 # coarse assumed heading-uncertainty growth +# per meter of odom-measured straight-line travel since the last full search +# -- not a real per-embodiment IMU/covariance model, just enough to keep the +# common short-interval attempt cheap (fan of 1) while still widening the +# search as the seed gets stale. Keyed on distance rather than clock time: +# new map area gets carved in roughly proportional to distance traveled, not +# elapsed time, so a stationary robot adds no new drift risk regardless of +# the clock. +TRACKING_AGE_BACKSTOP = 15.0 # seconds -- if distance-based sizing alone would +# keep the fan at a single candidate (robot appears stationary in odom) but +# this much time has passed anyway, still try a small residual fan. Catches +# "picked up and moved while appearing stationary," which distance can't see. + + +# Yaw-fan sizing history (design decisions kept as comments, not dead code): +# v1: fixed fan (YAW_FAN_DEG) tried on every attempt regardless of +# staleness. Wasteful: heading drift over a couple of seconds is +# normally well under a degree, so 0deg alone clears almost every +# short-interval attempt. +# v2: fan widened with elapsed time since last_good, centered at 0. +# v3: fan centered on odom's own measured heading change instead of 0. +# REGRESSED badly (yaw errors up to ~100deg in eval): a real robot +# turn is already correctly represented in the accumulated map via +# per-frame odometry placement -- the world_raw frame itself doesn't +# rotate -- so centering the search on "how much did the robot turn" +# actively rotates a good seed by however much the robot legitimately +# turned. `odom.orientation` is body-in-world_raw; `last_good.T`'s +# rotation is world_raw-in-map -- different frame pairing, not +# interchangeable. +# v4: reverted to center=0; width driven by distance/time since +# `last_good`. Self-defeating: `last_good` updates on every tracking +# success too, so this reset every ~2s hop and the fan almost never +# actually widened across a streak even as true cumulative drift grew. +# current: width driven by distance/time since the last FULL SEARCH +# instead (only resets on an absolute reacquisition), so it widens +# correctly across a whole tracking streak. +def yaw_fan_for_travel(distance_m: float, age_s: float) -> tuple[float, ...]: + """Yaw offsets to try, widening with odom-measured straight-line distance + traveled since the last full search (primary driver), plus a small + time-based backstop (`TRACKING_AGE_BACKSTOP`) for the "picked up while + appearing stationary" case distance alone can't see. Always centered at + 0 -- see the version history above. Public (no leading underscore) so + `eval.py` can reuse the exact same logic the live module runs, instead + of a parallel copy that could drift. + """ + step = ASSUMED_YAW_DRIFT_DEG_PER_M * distance_m + if age_s > TRACKING_AGE_BACKSTOP: + step = max(step, 2.0) + if step < 1.0: + return (0.0,) + if step < 5.0: + return (0.0, step, -step) + return YAW_FAN_DEG + + +def should_force_full_search( + now_ts: float, + last_good: tuple[np.ndarray, float, float] | None, + miss_count: int, + last_full_search_ts: float, + last_full_search_pos: np.ndarray | None, + latest_odom_xy: np.ndarray | None, +) -> bool: + """Decide SEARCHING (full relocalize()) vs TRACKING (seeded track()). + + Free function, not a method, so `eval.py`'s tracking-mode benchmark can + exercise the exact same gating logic the live module runs, instead of a + parallel reimplementation that could silently diverge. `now_ts` should + be the message's own timestamp, not wall-clock time, so this behaves + the same live and in replay/eval. + """ + if last_good is None: + return True + _, _, last_good_ts = last_good + if now_ts - last_good_ts > MAX_TRACKING_AGE: + return True + if miss_count >= MAX_TRACKING_MISSES: + return True + if now_ts - last_full_search_ts > SANITY_CHECK_INTERVAL: + return True + if latest_odom_xy is not None and last_full_search_pos is not None: + # Straight-line displacement, not accumulated path length -- a cheap + # proxy for "how much new map area has likely been carved since the + # last full search," not a real odometry integral. + if float(np.linalg.norm(latest_odom_xy - last_full_search_pos)) > SANITY_CHECK_DISTANCE: + return True + return False + class Config(ModuleConfig): map_file: str | None = ( @@ -49,12 +167,14 @@ class Config(ModuleConfig): ) publish_loaded_map: bool = False fitness_threshold: float = 0.45 + tracking_fitness_threshold: float = 0.5 use_carving: bool = True class RelocalizationModule(Module): config: Config global_map: In[PointCloud2] + odom: In[PoseStamped] loaded_map: Out[PointCloud2] merged_map: Out[PointCloud2] @@ -64,6 +184,13 @@ def __init__(self, **kwargs: Any) -> None: self._last_skip_log = 0.0 self._world_to_map: Subject[Transform | None] = Subject() + # Tracking-mode state (see module constants above + relocalize.track()). + self._latest_odom: PoseStamped | None = None + self._last_good: tuple[np.ndarray, float, float] | None = None # (T, fitness, ts) + self._miss_count: int = 0 + self._last_full_search_ts: float = 0.0 + self._last_full_search_pos: np.ndarray | None = None # xy at last full search + @rpc def start(self) -> None: super().start() @@ -76,6 +203,8 @@ def start(self) -> None: self._premap = PointCloud2.lcm_decode(path.read_bytes()) self._premap.frame_id = FRAME_MAP + self.register_disposable(self.odom.observable().subscribe(self._on_odom)) # type: ignore[no-untyped-call] + self.register_disposable( backpressure( self.global_map.observable().pipe( # type: ignore[no-untyped-call] @@ -126,25 +255,90 @@ def _publish_tf(self, tf: Transform | None) -> None: return self._world_to_map.on_next(tf) + def _on_odom(self, msg: PoseStamped) -> None: + self._latest_odom = msg + + def _should_force_full_search(self, now_ts: float) -> bool: + """Decide SEARCHING (full relocalize()) vs TRACKING (seeded track()). + + Thin wrapper around the free function `should_force_full_search` + above -- see its docstring for why this isn't inlined. + """ + latest_odom_xy = ( + np.array([self._latest_odom.position.x, self._latest_odom.position.y]) + if self._latest_odom is not None + else None + ) + return should_force_full_search( + now_ts, + self._last_good, + self._miss_count, + self._last_full_search_ts, + self._last_full_search_pos, + latest_odom_xy, + ) + def _try_relocalize(self, msg: PointCloud2) -> Transform | None: assert self._premap is not None t0 = time.monotonic() + + # Mode switch: cheap seeded tracking vs. the untouched global search. + # See `yaw_fan_for_travel`'s docstring for the design history behind + # how the tracking-mode seed/fan is computed. + force_full = self._should_force_full_search(msg.ts) try: - T, fitness = _relocalize(self._premap.pointcloud, msg.pointcloud) + if not force_full and self._last_good is not None: + seed_T, _, last_good_ts = self._last_good + distance_since_full_search = 0.0 + if self._latest_odom is not None and self._last_full_search_pos is not None: + now_xy = np.array([self._latest_odom.position.x, self._latest_odom.position.y]) + distance_since_full_search = float( + np.linalg.norm(now_xy - self._last_full_search_pos) + ) + fan = yaw_fan_for_travel( + distance_since_full_search, msg.ts - self._last_full_search_ts + ) + T, fitness = _track( + self._premap.pointcloud, msg.pointcloud, seed_T, yaw_fan_deg=fan + ) + mode = "tracking" + threshold = self.config.tracking_fitness_threshold + else: + T, fitness = _relocalize(self._premap.pointcloud, msg.pointcloud) + mode = "search" + threshold = self.config.fitness_threshold except Exception: logger.exception("relocalize() failed") return None dt = time.monotonic() - t0 n_pts = len(msg) - if fitness < self.config.fitness_threshold: - logger.warning( - f"relocalize rejected: fitness={fitness:.3f} < threshold={self.config.fitness_threshold} " - f"time_cost={dt:.1f}s n_pts={n_pts}" - ) + if fitness < threshold: + if mode == "tracking": + self._miss_count += 1 + logger.warning( + f"tracking miss ({self._miss_count}/{MAX_TRACKING_MISSES}): " + f"fitness={fitness:.3f} < threshold={threshold} time_cost={dt:.1f}s n_pts={n_pts}" + ) + else: + logger.warning( + f"relocalize rejected: fitness={fitness:.3f} < threshold={threshold} " + f"time_cost={dt:.1f}s n_pts={n_pts}" + ) return None - # relocalize(scan, map) returns T such that scan_in_map_frame = T(scan_raw). + # Carry state forward so the next attempt can use tracking mode, and + # so periodic sanity checks know when/where the last full search was. + self._miss_count = 0 + self._last_good = (T, fitness, msg.ts) + if mode == "search": + self._last_full_search_ts = msg.ts + if self._latest_odom is not None: + self._last_full_search_pos = np.array( + [self._latest_odom.position.x, self._latest_odom.position.y] + ) + + # relocalize()/track() return T such that scan_in_map_frame = T(scan_raw). # We are publishing a TF for map_in_scan_frame, notice that the base frame is `world` # so inverse the transform T here to get map_in_scan_frame T_inv = np.linalg.inv(T) @@ -155,7 +349,7 @@ def _try_relocalize(self, msg: PointCloud2) -> Transform | None: child_frame_id=FRAME_MAP, ) logger.info( - f"relocalize: fitness={fitness:.3f} time_cost={dt:.1f}s n_pts={n_pts} " + f"relocalize[{mode}]: fitness={fitness:.3f} time_cost={dt:.1f}s n_pts={n_pts} " f"reloc_t={T[:3, 3].round(3).tolist()} " f"TF {FRAME_WORLD!r} -> {FRAME_MAP!r} " f"published_t={T_inv[:3, 3].round(3).tolist()} " diff --git a/dimos/mapping/relocalization/plot_comparison.py b/dimos/mapping/relocalization/plot_comparison.py new file mode 100644 index 0000000000..ff1efaa3a1 --- /dev/null +++ b/dimos/mapping/relocalization/plot_comparison.py @@ -0,0 +1,200 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Parse two `eval.py` run logs (--mode global vs --mode tracking) into a +comparison table + plots. Doesn't re-run anything itself -- see `compare.sh` +for the end-to-end pipeline (run both modes, then call this). + +Usage: + uv run python -m dimos.mapping.relocalization.plot_comparison \ + --global-log /tmp/reloc_comparison/global.log \ + --tracking-log /tmp/reloc_comparison/tracking.log \ + --out-dir /tmp/reloc_comparison +""" + +from __future__ import annotations + +import re +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt # noqa: E402 +import numpy as np # noqa: E402 +import pandas as pd # noqa: E402 +import typer # noqa: E402 + +LINE_RE = re.compile( + r"t=\s*(?P[\d.]+)\s+" + r"n_pts=\s*(?P\d+)\s+" + r"mode=(?P\S+)\s+" + r"fitness=(?P[\d.]+)\s+" + r"wall=\s*(?P[\d.]+)s\s+" + r"cpu=\s*(?P[\d.]+)s\s+" + r"trans_err=(?P[\d.]+)m\s+" + r"yaw_err=\s*(?P[\-\d.]+)deg\s+" + r"(?PACCEPT|reject)" +) + +COLORS = {"search": "tab:red", "tracking": "tab:blue"} + + +def parse_log(path: Path) -> pd.DataFrame: + """Extract one row per attempt line; silently skips PGO/log-closure noise.""" + rows = [] + for line in path.read_text().splitlines(): + m = LINE_RE.search(line) + if not m: + continue + d = m.groupdict() + rows.append( + { + "ts": float(d["ts"]), + "n_pts": int(d["n_pts"]), + "mode": d["mode"], + "fitness": float(d["fitness"]), + "wall_s": float(d["wall_s"]), + "cpu_s": float(d["cpu_s"]), + "trans_err_m": float(d["trans_err_m"]), + "yaw_err_deg": float(d["yaw_err_deg"]), + "accepted": d["status"] == "ACCEPT", + } + ) + if not rows: + raise SystemExit(f"No attempt lines parsed from {path} -- wrong file, or eval.py's log format changed?") + df = pd.DataFrame(rows) + df["idx"] = range(len(df)) + return df + + +def _med_p90(df: pd.DataFrame, col: str, *, accepted_only: bool) -> tuple[float, float]: + vals = df.loc[df["accepted"], col] if accepted_only else df[col] + return float(vals.median()), float(np.percentile(vals, 90)) + + +def print_table(global_df: pd.DataFrame, tracking_df: pd.DataFrame) -> None: + groups = { + "global (baseline)": global_df, + "tracking - overall": tracking_df, + "tracking - search only": tracking_df[tracking_df["mode"] == "search"], + "tracking - tracking only": tracking_df[tracking_df["mode"] == "tracking"], + } + print() + header = ( + f"{'group':<28} {'n':>4} {'wall med/p90 (s)':>20} {'cpu med/p90 (s)':>20} " + f"{'trans_err med/p90 (m)':>24} {'yaw_err med/p90 (deg)':>24}" + ) + print(header) + print("-" * len(header)) + for name, g in groups.items(): + if g.empty: + continue + wall_med, wall_p90 = _med_p90(g, "wall_s", accepted_only=False) + cpu_med, cpu_p90 = _med_p90(g, "cpu_s", accepted_only=False) + te_med, te_p90 = _med_p90(g, "trans_err_m", accepted_only=True) + ye_med, ye_p90 = _med_p90(g, "yaw_err_deg", accepted_only=True) + print( + f"{name:<28} {len(g):>4} " + f"{wall_med:>9.2f}/{wall_p90:<9.2f} " + f"{cpu_med:>9.2f}/{cpu_p90:<9.2f} " + f"{te_med:>10.3f}/{te_p90:<12.3f} " + f"{ye_med:>10.2f}/{ye_p90:<12.2f}" + ) + + print() + total_global = global_df["wall_s"].sum() + total_tracking = tracking_df["wall_s"].sum() + reduction = 100 * (1 - total_tracking / total_global) + print(f"total wall-clock, {len(global_df)} attempts each:") + print(f" global: {total_global:.1f}s") + print(f" tracking: {total_tracking:.1f}s") + print(f" -> {reduction:.0f}% reduction in total wall-clock time") + + +def make_plots(global_df: pd.DataFrame, tracking_df: pd.DataFrame, out_dir: Path) -> None: + out_dir.mkdir(parents=True, exist_ok=True) + + # 1. Per-attempt wall-clock latency (log scale -- dynamic range is ~300x). + fig, ax = plt.subplots(figsize=(8, 4.5)) + ax.plot(global_df["idx"], global_df["wall_s"], "o-", color="tab:gray", label="global (baseline)") + ax.plot(tracking_df["idx"], tracking_df["wall_s"], "-", color="tab:blue", alpha=0.3) + for mode, g in tracking_df.groupby("mode"): + ax.scatter(g["idx"], g["wall_s"], color=COLORS.get(mode, "black"), label=f"tracking run: {mode}", zorder=3) + ax.set_yscale("log") + ax.set_xlabel("attempt #") + ax.set_ylabel("wall-clock latency (s, log scale)") + ax.set_title("Per-attempt latency: global search vs. mode-switched tracking") + ax.legend() + fig.tight_layout() + fig.savefig(out_dir / "latency_per_attempt.png", dpi=150) + plt.close(fig) + + # 2. Per-attempt translation error vs. ground truth. + fig, ax = plt.subplots(figsize=(8, 4.5)) + ax.plot(global_df["idx"], global_df["trans_err_m"], "o-", color="tab:gray", label="global (baseline)") + ax.plot(tracking_df["idx"], tracking_df["trans_err_m"], "-", color="tab:blue", alpha=0.3) + for mode, g in tracking_df.groupby("mode"): + ax.scatter(g["idx"], g["trans_err_m"], color=COLORS.get(mode, "black"), label=f"tracking run: {mode}", zorder=3) + ax.set_xlabel("attempt #") + ax.set_ylabel("translation error vs. ground truth (m)") + ax.set_title("Per-attempt pose error: global search vs. mode-switched tracking") + ax.legend() + fig.tight_layout() + fig.savefig(out_dir / "trans_err_per_attempt.png", dpi=150) + plt.close(fig) + + # 3. Total wall-clock time, bar chart. + fig, ax = plt.subplots(figsize=(5, 4.5)) + totals = [global_df["wall_s"].sum(), tracking_df["wall_s"].sum()] + bars = ax.bar(["global\n(baseline)", "tracking\n(our method)"], totals, color=["tab:gray", "tab:blue"]) + ax.set_ylabel("total wall-clock time (s)") + ax.set_title(f"Total time, {len(global_df)} attempts") + for b, v in zip(bars, totals, strict=True): + ax.text(b.get_x() + b.get_width() / 2, v, f"{v:.0f}s", ha="center", va="bottom") + reduction = 100 * (1 - totals[1] / totals[0]) + ax.text(0.5, 0.9, f"-{reduction:.0f}%", transform=ax.transAxes, ha="center", fontsize=16, color="tab:blue") + fig.tight_layout() + fig.savefig(out_dir / "total_wall_time.png", dpi=150) + plt.close(fig) + + # 4. Does self-reported fitness track real pose error? + fig, ax = plt.subplots(figsize=(6, 4.5)) + ax.scatter(global_df["fitness"], global_df["trans_err_m"], color="tab:gray", label="global (baseline)") + for mode, g in tracking_df.groupby("mode"): + ax.scatter(g["fitness"], g["trans_err_m"], color=COLORS.get(mode, "black"), label=f"tracking run: {mode}") + ax.set_xlabel("ICP fitness (self-reported)") + ax.set_ylabel("translation error vs. ground truth (m)") + ax.set_title("Does fitness track real pose error?") + ax.legend() + fig.tight_layout() + fig.savefig(out_dir / "fitness_vs_trans_err.png", dpi=150) + plt.close(fig) + + print(f"\nplots written to {out_dir}/") + + +def main( + global_log: str = typer.Option(..., "--global-log", help="Log from a --mode global eval.py run"), + tracking_log: str = typer.Option(..., "--tracking-log", help="Log from a --mode tracking eval.py run"), + out_dir: str = typer.Option("/tmp/reloc_comparison", "--out-dir"), +) -> None: + global_df = parse_log(Path(global_log)) + tracking_df = parse_log(Path(tracking_log)) + print_table(global_df, tracking_df) + make_plots(global_df, tracking_df, Path(out_dir)) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/mapping/relocalization/relocalize.py b/dimos/mapping/relocalization/relocalize.py index afab407494..44b4ead50f 100644 --- a/dimos/mapping/relocalization/relocalize.py +++ b/dimos/mapping/relocalization/relocalize.py @@ -22,6 +22,7 @@ import numpy as np import open3d as o3d # type: ignore[import-untyped] +from scipy.spatial.transform import Rotation _reg = o3d.pipelines.registration @@ -37,6 +38,12 @@ RERANK_DIST = FINE_VOXEL * 1.5 # inlier dist for fine-scale candidate scoring GRAVITY_TILT_MAX_DEG = 10.0 # reject candidates whose z-axis tilts more than this +# Tracking mode (`track()`): no FPFH/RANSAC, just a seeded ICP on wall-only +# clouds. Correspondence distance is looser than RERANK_DIST since the seed +# is a reused last-good transform, not a RANSAC-refined one. +TRACKING_CORR_DIST = 0.3 +TRACKING_MAX_ICP_ITER = 50 + def _preprocess( pcd: o3d.geometry.PointCloud, voxel_size: float @@ -120,6 +127,25 @@ def _gravity_tilt_deg(T: np.ndarray) -> float: return float(np.degrees(np.arccos(np.clip(z_world[2], -1.0, 1.0)))) +def _wall_subset(cloud: o3d.geometry.PointCloud) -> o3d.geometry.PointCloud: + """Points with roughly-horizontal normals (walls), excluding floor/ceiling. + + Floor/ceiling points have vertical normals; they fit equally well in any + yaw rotation (flat planes are rotationally symmetric), so a wrong-yaw + candidate can hide its wall misalignment behind a perfect floor fit. + Falls back to the full cloud if too few wall points survive (sparse/open + scenes). + """ + nrm = np.asarray(cloud.normals) + mask = np.abs(nrm[:, 2]) < 0.7 # roughly horizontal + if mask.sum() < 100: + return cloud + sub = o3d.geometry.PointCloud() + sub.points = o3d.utility.Vector3dVector(np.asarray(cloud.points)[mask]) + sub.normals = o3d.utility.Vector3dVector(nrm[mask]) + return sub + + def relocalize( global_map: o3d.geometry.PointCloud, local_map: o3d.geometry.PointCloud, @@ -167,22 +193,13 @@ def relocalize( upright = [T for T in candidates if _gravity_tilt_deg(T) <= GRAVITY_TILT_MAX_DEG] pool = upright if upright else candidates - # Build WALL-ONLY clouds for scoring + polish. Floor/ceiling points have - # vertical normals; they fit equally well in any yaw rotation (flat planes - # are rotationally symmetric). Including them in scoring lets a 180°-flipped - # candidate hide its wall misalignment behind perfect floor alignment. The - # FULL clouds are still used for the final refinement, so the gravity - # anchor and inlier density are preserved in the output. - def _wall_subset(cloud: o3d.geometry.PointCloud) -> o3d.geometry.PointCloud: - nrm = np.asarray(cloud.normals) - mask = np.abs(nrm[:, 2]) < 0.7 # roughly horizontal - if mask.sum() < 100: - return cloud # too sparse -> fall back to full cloud - sub = o3d.geometry.PointCloud() - sub.points = o3d.utility.Vector3dVector(np.asarray(cloud.points)[mask]) - sub.normals = o3d.utility.Vector3dVector(nrm[mask]) - return sub - + # Build WALL-ONLY clouds for scoring + polish (see `_wall_subset` above, + # hoisted to module level so `track()` can reuse it too). Floor/ceiling + # points have vertical normals; they fit equally well in any yaw rotation + # (flat planes are rotationally symmetric). Including them in scoring lets + # a 180°-flipped candidate hide its wall misalignment behind perfect + # floor alignment. The FULL clouds are still used for the final + # refinement, so the gravity anchor and inlier density are preserved. src_walls = _wall_subset(src_fine) tgt_walls = _wall_subset(tgt_fine) @@ -220,3 +237,71 @@ def fine_fitness(T: np.ndarray) -> float: _reg.ICPConvergenceCriteria(max_iteration=50), ) return np.asarray(final.transformation), best_fit + + +def track( + global_map: o3d.geometry.PointCloud, + local_map: o3d.geometry.PointCloud, + seed_T: np.ndarray, + *, + yaw_fan_deg: tuple[float, ...] = (0.0,), + correspondence_dist: float = TRACKING_CORR_DIST, + max_iterations: int = TRACKING_MAX_ICP_ITER, +) -> tuple[np.ndarray, float]: + """Cheap seeded ICP for tracking mode: no FPFH, no RANSAC. + + `seed_T` is reused directly from the last accepted `relocalize()`/ + `track()` result (same convention: ``local_in_global = seed_T @ + local_raw``) — the caller decides whether that's still a reasonable + starting point (recency, miss count, drift/sanity checks) before calling + this. `yaw_fan_deg` tries a small number of yaw offsets around `seed_T` + (0 first) since heading error rotates the whole cloud and is the + dimension most likely to have drifted since the seed was computed; + scored by wall-only fitness (same symmetry-breaking trick as + `relocalize()`'s rerank, reused via `_wall_subset`) so a wrong-yaw + candidate can't hide behind a good floor/ceiling fit. + + Returns ``(best_T, best_wall_fitness)`` — caller applies its own accept + threshold, same as `relocalize()`. + """ + src_fine = local_map.voxel_down_sample(FINE_VOXEL) + src_fine.estimate_normals( + o3d.geometry.KDTreeSearchParamHybrid(radius=FINE_VOXEL * 2, max_nn=30) + ) + tgt_fine = _global_fine(global_map, FINE_VOXEL) + + src_walls = _wall_subset(src_fine) + tgt_walls = _wall_subset(tgt_fine) + + # Yaw offsets are applied about the body cloud's own centroid (in the + # local/body frame, before seed_T maps it into the global frame) — same + # "rotate about centroid, not origin" trick as relocalize()'s yaw-flip + # variant, so a perturbed candidate stays near the seed's location + # instead of swinging the whole cloud across the map. + src_pts = np.asarray(src_fine.points) + c_body = np.array([src_pts[:, 0].mean(), src_pts[:, 1].mean(), 0.0]) + + tukey = _reg.TransformationEstimationPointToPlane(_reg.TukeyLoss(k=correspondence_dist)) + + best_fit = -1.0 + best_T = seed_T + for dyaw in yaw_fan_deg: + rz = np.eye(4) + rz[:3, :3] = Rotation.from_euler("z", dyaw, degrees=True).as_matrix() + t_pivot = np.eye(4) + t_pivot[:3, 3] = c_body - rz[:3, :3] @ c_body + seed_variant = seed_T @ (t_pivot @ rz) + + r = _reg.registration_icp( + src_walls, + tgt_walls, + correspondence_dist, + seed_variant, + tukey, + _reg.ICPConvergenceCriteria(max_iteration=max_iterations), + ) + if r.fitness > best_fit: + best_fit = float(r.fitness) + best_T = np.asarray(r.transformation) + + return best_T, best_fit diff --git a/dimos/mapping/relocalization/results/global.log b/dimos/mapping/relocalization/results/global.log new file mode 100644 index 0000000000..e58a0f59d4 --- /dev/null +++ b/dimos/mapping/relocalization/results/global.log @@ -0,0 +1,65 @@ +warning: `VIRTUAL_ENV=/home/nibarkavi/Desktop/Rotate8/venv` does not match the project environment path `.venv` and will be ignored; use `--active` to target the active environment instead +running PGO for ground truth... +04:51:34.716 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.063 source=125 target=78 +04:51:35.015 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.021 source=134 target=96 +04:51:38.448 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0421 source=289 target=248 +04:51:38.767 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0173 source=298 target=240 +04:51:39.167 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0548 source=308 target=68 +04:51:39.572 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0386 source=317 target=60 +04:51:39.962 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.023 source=324 target=50 +04:51:40.439 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0337 source=333 target=38 +04:51:40.825 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0395 source=341 target=14 +04:51:41.358 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0404 source=353 target=16 +04:51:43.678 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.057 source=435 target=404 +04:51:44.151 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0342 source=442 target=415 +04:51:44.747 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0297 source=452 target=407 +04:51:47.167 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.1571 source=539 target=498 +04:51:47.817 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0499 source=547 target=499 +04:51:48.544 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0445 source=562 target=498 +04:51:49.245 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0477 source=576 target=498 +04:51:49.975 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0274 source=588 target=490 +04:51:50.560 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0244 source=596 target=485 +04:51:51.156 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0403 source=605 target=481 +04:51:54.270 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0173 source=710 target=668 +04:51:54.930 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0499 source=719 target=668 +04:51:55.588 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0491 source=726 target=666 +04:51:57.323 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0193 source=772 target=734 +04:51:58.121 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.013 source=782 target=741 +04:51:59.058 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0143 source=798 target=741 +04:51:59.941 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0207 source=808 target=734 +04:52:00.700 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0247 source=815 target=727 +04:52:01.417 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0217 source=825 target=720 +04:52:03.854 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.09 source=898 target=10 +04:52:04.713 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0457 source=911 target=10 +04:52:05.552 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0419 source=922 target=31 +04:52:06.427 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.04 source=932 target=32 +04:52:07.181 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0243 source=940 target=332 +04:52:07.889 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0108 source=947 target=54 +04:52:08.871 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0128 source=959 target=55 +04:52:09.689 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0077 source=971 target=61 +04:52:10.465 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0089 source=982 target=74 +04:52:11.258 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0109 source=991 target=93 +04:52:12.062 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0085 source=1000 target=100 +PGO ground truth ready in 41.4s (1007 keyframes, 40 loop closures) +t=1778055152.52 n_pts= 50106 mode=search fitness=0.725 wall=14.01s cpu=53.22s trans_err=0.338m yaw_err= 1.76deg ACCEPT +t=1778055154.60 n_pts= 52009 mode=search fitness=0.722 wall=13.67s cpu=48.55s trans_err=0.344m yaw_err= 1.69deg ACCEPT +t=1778055156.67 n_pts= 55566 mode=search fitness=0.750 wall=13.74s cpu=49.40s trans_err=0.349m yaw_err= 1.59deg ACCEPT +t=1778055158.77 n_pts= 56370 mode=search fitness=0.762 wall=13.75s cpu=43.39s trans_err=0.357m yaw_err= 1.70deg ACCEPT +t=1778055160.83 n_pts= 61314 mode=search fitness=0.771 wall=14.76s cpu=47.36s trans_err=0.366m yaw_err= 2.11deg ACCEPT +t=1778055162.92 n_pts= 65783 mode=search fitness=0.793 wall=15.67s cpu=47.26s trans_err=0.369m yaw_err= 1.98deg ACCEPT +t=1778055164.98 n_pts= 68931 mode=search fitness=0.795 wall=15.92s cpu=46.99s trans_err=0.372m yaw_err= 1.89deg ACCEPT +t=1778055167.07 n_pts= 73541 mode=search fitness=0.795 wall=16.08s cpu=48.06s trans_err=0.374m yaw_err= 1.83deg ACCEPT +t=1778055169.13 n_pts= 77785 mode=search fitness=0.799 wall=16.55s cpu=49.33s trans_err=0.372m yaw_err= 1.78deg ACCEPT +t=1778055171.16 n_pts= 83485 mode=search fitness=0.792 wall=16.03s cpu=47.74s trans_err=0.379m yaw_err= 1.80deg ACCEPT +t=1778055173.17 n_pts= 88372 mode=search fitness=0.779 wall=21.35s cpu=62.08s trans_err=0.392m yaw_err= 1.89deg ACCEPT +t=1778055175.24 n_pts= 92576 mode=search fitness=0.776 wall=17.89s cpu=52.91s trans_err=0.402m yaw_err= 1.92deg ACCEPT +t=1778055177.32 n_pts= 94790 mode=search fitness=0.770 wall=18.20s cpu=53.14s trans_err=0.416m yaw_err= 1.97deg ACCEPT +t=1778055179.39 n_pts= 98626 mode=search fitness=0.785 wall=18.79s cpu=53.57s trans_err=0.428m yaw_err= 1.99deg ACCEPT +t=1778055181.47 n_pts= 102917 mode=search fitness=0.802 wall=19.72s cpu=55.84s trans_err=0.442m yaw_err= 2.07deg ACCEPT + +-- overall (n=15) -- +accepted=15 (100%) +latency wall median=16.03s p90=19.35s +latency cpu median=49.33s p90=54.93s +trans_err (accepted) median=0.372m p90=0.423m +yaw_err (accepted) median=1.89deg p90=2.04deg diff --git a/dimos/mapping/relocalization/results/tracking.log b/dimos/mapping/relocalization/results/tracking.log new file mode 100644 index 0000000000..7a0fc5ae9d --- /dev/null +++ b/dimos/mapping/relocalization/results/tracking.log @@ -0,0 +1,79 @@ +warning: `VIRTUAL_ENV=/home/nibarkavi/Desktop/Rotate8/venv` does not match the project environment path `.venv` and will be ignored; use `--active` to target the active environment instead +running PGO for ground truth... +04:56:29.528 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.063 source=125 target=78 +04:56:29.840 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.021 source=134 target=96 +04:56:33.342 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0421 source=289 target=248 +04:56:33.671 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0173 source=298 target=240 +04:56:34.086 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0548 source=308 target=68 +04:56:34.499 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0386 source=317 target=60 +04:56:34.930 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.023 source=324 target=50 +04:56:35.351 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0337 source=333 target=38 +04:56:35.795 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0392 source=341 target=14 +04:56:36.325 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0399 source=353 target=16 +04:56:38.668 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.058 source=435 target=404 +04:56:39.146 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0341 source=442 target=415 +04:56:39.721 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.03 source=452 target=407 +04:56:42.130 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.1579 source=539 target=498 +04:56:42.790 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0511 source=547 target=499 +04:56:43.614 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0447 source=562 target=498 +04:56:44.320 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0481 source=576 target=498 +04:56:44.984 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0274 source=588 target=490 +04:56:45.545 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0231 source=596 target=485 +04:56:46.158 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0403 source=605 target=481 +04:56:49.203 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0162 source=710 target=668 +04:56:49.871 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0472 source=719 target=668 +04:56:50.550 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0745 source=726 target=666 +04:56:52.413 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.019 source=772 target=734 +04:56:53.173 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0127 source=782 target=741 +04:56:54.200 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0144 source=798 target=741 +04:56:55.078 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0199 source=808 target=734 +04:56:55.850 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0233 source=815 target=727 +04:56:56.637 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0217 source=825 target=720 +04:56:59.059 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0973 source=898 target=10 +04:56:59.974 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0497 source=911 target=10 +04:57:00.810 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0424 source=922 target=31 +04:57:01.703 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0403 source=932 target=32 +04:57:02.474 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0243 source=940 target=332 +04:57:03.194 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0108 source=947 target=54 +04:57:04.237 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0129 source=959 target=55 +04:57:05.129 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0076 source=971 target=61 +04:57:05.930 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0091 source=982 target=74 +04:57:06.731 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0106 source=991 target=93 +04:57:07.543 [inf][os/mapping/loop_closure/pgo.py] Loop closure detected score=0.0085 source=1000 target=100 +PGO ground truth ready in 42.1s (1007 keyframes, 40 loop closures) +t=1778055152.52 n_pts= 50106 mode=search fitness=0.725 wall=14.56s cpu=56.45s trans_err=0.341m yaw_err= 1.77deg ACCEPT +t=1778055154.60 n_pts= 52009 mode=tracking fitness=0.924 wall= 0.03s cpu= 0.26s trans_err=0.365m yaw_err= 2.03deg ACCEPT +t=1778055156.67 n_pts= 55566 mode=tracking fitness=0.931 wall= 0.03s cpu= 0.25s trans_err=0.374m yaw_err= 2.06deg ACCEPT +t=1778055158.77 n_pts= 56370 mode=search fitness=0.761 wall=14.87s cpu=46.67s trans_err=0.357m yaw_err= 1.71deg ACCEPT +t=1778055160.83 n_pts= 61314 mode=tracking fitness=0.943 wall= 0.06s cpu= 0.46s trans_err=0.403m yaw_err= 2.48deg ACCEPT +t=1778055162.92 n_pts= 65783 mode=tracking fitness=0.947 wall= 0.05s cpu= 0.35s trans_err=0.403m yaw_err= 2.31deg ACCEPT +t=1778055164.98 n_pts= 68931 mode=search fitness=0.795 wall=16.26s cpu=49.94s trans_err=0.373m yaw_err= 1.90deg ACCEPT +t=1778055167.07 n_pts= 73541 mode=tracking fitness=0.948 wall= 0.05s cpu= 0.42s trans_err=0.403m yaw_err= 2.12deg ACCEPT +t=1778055169.13 n_pts= 77785 mode=tracking fitness=0.949 wall= 0.05s cpu= 0.37s trans_err=0.397m yaw_err= 2.06deg ACCEPT +t=1778055171.16 n_pts= 83485 mode=search fitness=0.792 wall=15.95s cpu=48.50s trans_err=0.379m yaw_err= 1.81deg ACCEPT +t=1778055173.17 n_pts= 88372 mode=tracking fitness=0.947 wall= 0.05s cpu= 0.41s trans_err=0.422m yaw_err= 2.41deg ACCEPT +t=1778055175.24 n_pts= 92576 mode=tracking fitness=0.955 wall= 0.06s cpu= 0.44s trans_err=0.431m yaw_err= 2.38deg ACCEPT +t=1778055177.32 n_pts= 94790 mode=tracking fitness=0.957 wall= 0.05s cpu= 0.37s trans_err=0.449m yaw_err= 2.57deg ACCEPT +t=1778055179.39 n_pts= 98626 mode=tracking fitness=0.955 wall= 0.06s cpu= 0.48s trans_err=0.451m yaw_err= 2.45deg ACCEPT +t=1778055181.47 n_pts= 102917 mode=search fitness=0.803 wall=19.93s cpu=57.33s trans_err=0.440m yaw_err= 2.08deg ACCEPT + +-- overall (n=15) -- +accepted=15 (100%) +latency wall median=0.06s p90=16.13s +latency cpu median=0.44s p90=53.85s +trans_err (accepted) median=0.403m p90=0.445m +yaw_err (accepted) median=2.08deg p90=2.47deg + +-- search (bootstrap / reacquire / sanity-check) (n=5) -- +accepted=5 (100%) +latency wall median=15.95s p90=18.46s +latency cpu median=49.94s p90=56.98s +trans_err (accepted) median=0.373m p90=0.415m +yaw_err (accepted) median=1.81deg p90=2.01deg + +-- tracking (seeded, cheap) (n=10) -- +accepted=10 (100%) +latency wall median=0.05s p90=0.06s +latency cpu median=0.39s p90=0.46s +trans_err (accepted) median=0.403m p90=0.449m +yaw_err (accepted) median=2.35deg p90=2.49deg