relocalization: measured lidar aligner, ground-truth eval, replay demo - #3890
Open
leshy wants to merge 27 commits into
Open
relocalization: measured lidar aligner, ground-truth eval, replay demo#3890leshy wants to merge 27 commits into
leshy wants to merge 27 commits into
Conversation
Eval takes a recording, a premap built from another stretch of it, and a window; ground truth is the identity, since both live in the recording's own frame. Probes are placed across the window and labelled by how much of each one the premap contains - a place it never saw is not excluded but is the negative half of the test, where the only right answer is a refusal. Scored on hit rate over answerable probes, false fixes over all of them, displacement when right, and latency plus CPU. Tuned with optuna through dimos/evals/tuning.py, which is a study helper and not a framework: the knobs are whatever the objective's suggest_* calls declare. RelocalizeConfig extracts every literal that was hardcoded in relocalize(), and the eval found three that mattered - oriented FPFH normals, wide-to-narrow ICP, and RANSAC restarts - taking 2/6 probes to 6/6 with no false fixes. Premap preprocessing is hoisted into prepare(), so a live module pays it once at start instead of on every fix.
A trial's score is one draw of `samples` probes against an unseeded RANSAC, so the trials topping a front are partly the lucky ones - the v9 study put 12 configs at 100% hit and 45 more one probe behind, with nothing to tell the two groups apart. `verify` re-runs each candidate N times for a rate worth trusting, then again on `probe_starts(half_step=True)`: the same count of starts over the same window, offset by half the spacing, so a config that only learned which handful of places are easy shows it. Also documents that optuna sits in the `dev` group while this project sets default-groups=["tests"], so a plain `uv run` uninstalls it as extraneous; `tune` and `verify` want `uv run --group dev`.
… origin _yaw_only rebuilt the rotation but kept the translation, which pivots the cloud about the world origin. These clouds sit 50-140 m from it, so half a degree of tilt moved them 0.36 m - already past the distance ICP looks - and two degrees moved them 1.44 m. The correction meant to clean up a hypothesis was destroying it. It shows up in the v9 study: 11 of the 12 configs that scored 100% hit with no false fixes had gravity_aligned off. The tuner was routing around this. Now the flattening keeps the cloud's centre where the hypothesis put it and removes only the tilt.
A probe was one guess at a fixed frame count. A robot does not work that way - it keeps listening while it thinks, so the scans that land during a failed match are the next attempt's evidence, for free. Attempts now repeat until a fix clears the cutoff or the frame budget runs out, and each one sees whatever the lidar has delivered by the time it starts: the clock sets the frame count, not a schedule. `frames` is no longer tuned. That prices the real tradeoff. A slower matcher gets fewer attempts in the same wall time but each sees more scans, and latency - now the whole wait, from the first scan to a fix or a refusal - charges for both. It also sharpens the negative set, which the window widening to 400 s extends to about a third of all probes: an unmatchable place now gets every attempt to be wrongly accepted rather than one, so the false-fix rate measures the retry loop's real risk instead of a single draw's.
RelocalizeConfig now carries trial 229 of the go2-sf-area1 study - the candidate that held its hit rate on probes it was never tuned against, where two others dropped forty points and quadrupled their false fixes. Its two neighbours on the front agree to within a few percent on every field, so this is a plateau rather than a spike. The notable moves against the old literals: ransac_iters 500k -> 1.58M, voxel_fine 0.5 -> 0.30, coarse_dist_factor 1.5 -> 2.73, and icp_stages 2 rather than a single pass, which is what lets a hypothesis landing a metre out still be reached. gravity_aligned goes back to False and ransac_restarts to 1, reverting defaults I set earlier off an ablation that held everything else at the old values: gravity and restarts were rescuing bad hypotheses that these FPFH settings do not produce. Post-fix, gravity measures as a wash here. Measured on one outdoor mid360 walk. Another sensor or a room-sized map wants its own study.
`relocalize()` now returns the Fix or None. Refusing is a real answer - the right one for a place the premap never saw - and the threshold that decides it moves onto RelocalizeConfig, beside the knobs it is inseparable from. A caller configures one object and checks whether it got a fix. The base module's duplicate gate goes with it. It was a second place to configure the same decision, on a field only the lidar path ever read, and two thresholds for one question drift apart. submit() now publishes what an implementation hands it. `align()` keeps the always-answers behaviour for callers that need to see a refused fix - the eval measures where rejected hypotheses actually landed, which is most of the diagnosis. gravity_aligned goes back on. It is simply true when both maps come from a lidar-inertial odometry, and it removes a degree of freedom the answer cannot use; the tuning measured it as a wash once the pivot bug was fixed, and a wash on a walk whose hypotheses rarely tilt is not evidence against the cases where they do. fitness_threshold ships at 0.5, in the gap between the two populations the eval measured: right-place fixes at 0.58-0.76, absent places at 0.13-0.17.
global_map/local_map, Prepared's fields and the registration results were all `Any`, leaving the reader to infer what to pass from a voxel_down_sample call three functions down. They are PointCloud, Feature and RegistrationResult, imported under TYPE_CHECKING since open3d stays a lazy import inside the functions - it is heavy, and a module-scope import would cost every process that merely touches this file. Needed `from __future__ import annotations`: Prepared is a NamedTuple, so its annotations were evaluated at import and a type-checking-only name raised NameError. open3d ships no stubs and is in mypy's ignore_missing_imports, so these widen to Any regardless - they document the contract, they do not enforce it.
align() took a global_map it ignored whenever `prepared` was passed - which every real caller does - and took a config that prepare() had already been given, so preparing at one voxel and matching at another was a thing you could silently do. Prepared becomes PreparedMap and holds the cloud, its derived forms and the config that produced them. align(premap, local_map) and relocalize(premap, local_map) then need nothing else: the derived forms only mean anything under the settings that built them, so those settings travel with them rather than being asked for again.
lidar/relocalize.py is now the algorithm - RelocalizeConfig, PreparedMap, Fix, prepare, align, relocalize - with no streams, no ports, no clock and no dimos runtime beyond a pydantic base. lidar/module.py keeps what a module is for: resolving the map file, throttling the cloud stream, and turning a Fix into a TF. The eval already only wanted the algorithm, and now says so in its imports.
gravity_aligned and _yaw_only were one feature - the flag existed to call the function - and the evidence for it does not hold up. The ablation that sold it held every other knob at the old values, so what it really measured was gravity rescuing hypotheses that the tuned FPFH settings do not produce. Measured honestly, after fixing the pivot bug it introduced, it was a wash. Removing it also returns the shipped defaults to exactly the configuration verify checked: trial 229 had gravity_aligned off, and turning it on was my override, not a measurement. Tilt stays in the eval as a diagnostic - a tilted answer is still provably wrong, which is worth seeing - it just no longer steers the aligner.
…nt floor LidarRelocalizer(global_map, config) replaces prepare()/PreparedMap and the free functions. The map's preprocessing is the pipeline's dominant cost and never changes between queries, so it belongs in a constructor; holding the config there too means the derived forms and the settings that built them cannot drift apart. Callers now build one and call .relocalize(local_map). RelocalizeConfig's numbers are scales - voxel sizes, radii, correspondence distances - so they belong to a rig, not to relocalization. PRESETS names them: `mid360` is what the go2-sf-area1 study measured, and a new sensor adds an entry instead of nudging that one. `eval run --preset` selects. min_local_points was 50_000, sized for a denser mapper. A mid360 sweep is ~2.8k points; two of them voxel down to ~3.5k, which relocalizes fine, and twenty frames reach only ~13k. The module would have skipped every cloud this rig produces. Now 1_000, which only rejects a nearly empty map. readme gains the tuning procedure end to end, including the two ways this preset was nearly got wrong: an ablation that moves one knob while the rest sit at stale values measures the interaction, and a threshold tuned for one call pattern does not transfer to another.
It went out on an interval from startup, regardless of whether anything had been matched. The premap is stamped in the `map` frame, and `map` does not exist until a fix relates it to `world`, so every one of those publishes before the first match sent a cloud whose frame nothing could resolve - a viewer either drops it or draws it at identity, which is worse. Gated with the same with_latest_from the base already uses to republish tf: it emits nothing until the first fix lands, and republishes for late subscribers after that.
The two module.py files had no stated boundary, so runtime bits sat on whichever one they were written into. Draw it at what every strategy shares: a prior map and a Fix, hence `tf` and `loaded_map`. Base: Fix, both Out ports, accept() to invert a fix into the world -> map transform, set_premap() to stamp the map frame and gate its republish on a fix. Lidar: the global_map input, the .pc2.lcm premap, min_local_points, reloc_interval, the relocalizer. Matching apriltags or GPS shares none of the latter, so none of it is in the base.
loaded_map is a PointCloud2 on every strategy, so the .pc2.lcm behind it is the base's concern too: map_file, the load, the map-frame stamp and the gated republish all move up, and set_premap() goes away with them. An implementation now reads self.premap after super().start() - None means no map was configured - and is left with only what the base cannot know: its inputs, when to attempt a fix, and how good one must be.
Implementations inherit this alongside their own module's methods, where a bare `accept` says nothing about what is being accepted.
The matrix was open3d's output shape leaking through the contract, and it left the frames implicit - accept_relocalization had to know which way round the matrix ran and stamp map/world onto it from the outside. The aligner stamps it now, so the direction travels with the value and submit's frame assertion doubles as a check that an implementation stamped its Fix correctly. accept_relocalization is one line. The eval, which does matrix arithmetic on the placement, calls to_matrix().
Inlier RMSE is ICP's and margin is the spread across RANSAC restarts; neither means anything to a GPS or apriltag fix, and defaulting them to 0.0 in the base only hid that. Fix is now a frozen dataclass of the two fields every strategy has, and LidarFix subclasses it with the two the pipeline actually produces. Nothing acts on either - accepting is still fitness against fitness_threshold.
The class carried mid360's measured numbers as field defaults, so a bare RelocalizeConfig() silently meant one particular Livox on one particular walk. The scales - voxel sizes, neighbourhood radii, correspondence distances - are now required fields, and MID360 spells all of them out under a name that says which rig they came from. LidarRelocalizer's config argument loses its default for the same reason. What keeps a default is the handful of search budgets and caps (max_nn, ransac_confidence/n, icp_max_iter): those are about how hard to look, not how big the world is, so they are not per-rig. Left the class name alone: eval.py builds one from optuna params while tuning whatever rig you point it at, and LidarConfig.relocalize annotates any rig's settings, so Mid360Config would be wrong in both places. The preset instance is where the rig's name belongs.
…st fix blueprints.py runs the stack without a robot: a RecordingPlayer replays a recording's registered scans at wall-clock rate (seek/duration/speed/loop) into the same VoxelGridMapper the Go2 stack uses, so LidarRelocalization meets the global_map it would on hardware. Recording and premap are the eval's dataset, so a demo that looks wrong and an eval that scores badly are the same bug. Measured on the mid360 walk: fix in 0.5 s, fitness 0.840, 0.14 m off identity. Watching it run showed the module re-matching forever after succeeding, so Config grows relocalize_once (default on): a premap fix does not go stale the way odometry does, the accepted TF is republished either way, and every further attempt is CPU plus a chance to overwrite a good answer with a worse one. keep_relocalizing() is on the base because that reasoning is not lidar's; the lidar module gates its input on it.
leshy
requested review from
Dreamsorcerer,
aclauer,
mustafab0,
paul-nechifor and
spomichter
as code owners
September 2, 2026 17:42
One caller, and the wrapper was mostly docstring - the parts worth keeping (studies resume by name, browse with optuna-dashboard) are already in lidar/readme.md. create_study with a seeded sampler and load_if_exists sits in `tune` where you can see it while reading the search space.
docs/capabilities/navigation/relocalization.md is the Go2 how-to, not this refactor's documentation. Back to main's version; what the split and the tuning look like is in lidar/readme.md and the modules' own docstrings.
Contributor
Greptile SummaryThis PR adds lidar relocalization evaluation, parameter tuning, holdout verification, and replay support. Runtime checks found that holdout verification uses fewer probes than the tuning set, custom recordings with Confidence Score: 3/5The evaluation and verification commands need correction before they can reliably assess or tune relocalization behavior. Focused runtime checks reproduced mismatched verification sample populations, invalid custom-window probe scheduling, and zero-sample crashes across the evaluation commands. Files Needing Attention:
 T-Rex LogsWhat T-Rex did
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
Comment on lines
+348
to
+350
| starts = np.linspace(lo, last, samples) | ||
| if half_step: | ||
| starts = starts[:-1] + np.diff(starts) / 2 |
Contributor
There was a problem hiding this comment.
When half_step=True, the final evenly spaced start is removed before shifting the remaining values. Consequently, verify --samples 2 runs two tuned probes but one holdout probe, while --samples 10 runs ten tuned probes but nine holdout probes. The verification result therefore compares unequal populations rather than the documented same-sized disjoint holdout set. Generate the requested number of shifted positions without dropping a probe.
Artifacts
Focused probe-starts runtime harness
- This exact executable harness loads the actual eval.py implementation while mocking only unavailable import dependencies and the dataset/store seam, ending with the takeaway.
Focused probe-starts runtime output
- This captured successful command output shows the actual implementation returning 1 holdout probe for samples=2 and 9 for samples=10, ending with the takeaway.
Comment on lines
+579
to
+582
| if window_from is not None or window_to is not None: | ||
| lo = window_from if window_from is not None else (window[0] if window else 0.0) | ||
| hi = window_to if window_to is not None else (window[1] if window else 1e9) | ||
| window = (lo, hi) |
Contributor
There was a problem hiding this comment.
For an unregistered recording, passing --from without --to stores 1e9 as the upper bound. Probe generation then treats it as a real recording window: on a 30-second recording it selects timestamps at 500000002.5 and 999999995.0 seconds, and map accumulation raises ValueError because those scans do not exist. Resolve an omitted upper bound from the lidar recording, or defer resolving it until probe_starts reads the stream range.
Artifacts
Controlled relocalization window reproduction source
- This exact executed harness invokes production registration, probe generation, and runtime accumulation against a 30-second controlled recording seam, and the takeaway is that the test directly covers the alleged path.
Observed current-code unbounded-window output
- This captured command output records the `1e9` window, probes at 500000002.5 and 999999995.0 seconds, and the accumulation ValueError, and the takeaway is that the reported defect occurs at runtime.
Comment on lines
+516
to
+521
| return ( | ||
| len(hits) / answerable if answerable else 0.0, | ||
| false_fixes / len(probes), | ||
| error, | ||
| latency, | ||
| float(np.median([p.cpu_s for p in probes])), |
Contributor
There was a problem hiding this comment.
run, tune, and verify accept --samples 0, which produces no probes. Each command subsequently calls summarize([]), where false_fixes / len(probes) raises ZeroDivisionError rather than returning a command-line validation error. Require at least one sample on each option and retain a defensive empty-probe check in summarize.
Artifacts
Executed zero-sample mock harness source
- This is the exact Python harness executed against the unchanged evaluation module to mock unavailable services and invoke every requested zero-sample path, with the takeaway that it reaches the production summarization logic.
Observed zero-sample command-path output
- This captured output from the executed harness shows empty probe lists and `ZeroDivisionError` for summarize, run, tune, and verify while every samples option has `min=None`, with the takeaway that zero is accepted and fails at runtime.
❌ 1 Tests Failed:
View the top 1 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
…up advice The file is about running and tuning the eval, so name it after that. Its install line was wrong in a way that bites: it said `uv run --group dev` for optuna, but asking for a group triggers a full sync, and a sync prunes maturin-built extensions - so it handed you optuna and took away dimos_voxel_ray_tracing, which every command in the file needs. Documented `VIRTUAL_ENV=$PWD/.venv uv pip install optuna` instead, which adds the package without resyncing, and verified `tune` runs end to end that way. optuna.db is gitignored; it is a local study store.
The prune only happens on the sync that first installs the group. Doing `uv sync --group dev` and *then* `maturin develop` leaves both in place, and every later invocation - with or without --group dev - has nothing to sync and leaves the extension alone. Verified end to end. Replaces the `uv pip install optuna` workaround, which sidestepped the sync instead of just ordering it.
Was a shape (`dimos map global <recording> --export`) rather than something you could run. The real one, with --seek 400 as the point of the exercise: the premap is the walk after 400 s, the probes come from before it, and only because the walk is a loop do the two halves overlap in space at all. --voxel 0.005 confirmed against the file - downsampling the shipped premap at 0.005 is a no-op, so that is its grid. Also corrects the DATASETS snippet, which still showed the old premap filename.
Rewrote to match the register Ivan used in his own edit to the file: short sentences, one idea each, no em dashes, ordinary words. Same content.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
dimos run relocalize-mid360 --seek 60readme, and eval guide is WIP, just sharing so SF team can integrate, reloc module/type spec is stable