Nav pt3: Enable PGO topics, Optimize PGO, and add KITTI-360 benchmark scaffolding#2099
Open
jeff-hykin wants to merge 67 commits into
Open
Nav pt3: Enable PGO topics, Optimize PGO, and add KITTI-360 benchmark scaffolding#2099jeff-hykin wants to merge 67 commits into
jeff-hykin wants to merge 67 commits into
Conversation
… rrb
Native module (cpp/main.cpp) now publishes two new streams on every
keyframe: GraphNodes3D for keyframe optimized poses, LineSegments3D for
odometry (traversability=1.0) and loop-closure (0.4) edges. Both wire
through SimplePGO::keyPoses() + historyPairs() — no changes needed to
simple_pgo.{h,cpp} since the accessors already exist. Native binary
rebuilt cleanly via nix build .#default --no-write-lock-file.
Python (pgo.py) declares matching pgo_graph_nodes / pgo_graph_edges Out
streams so the rerun bridge auto-discovers and logs them.
nav_stack_rerun_config() now picks _agentic_debug_rerun_blueprint when
agentic_debug=True — an rrb.Horizontal layout with a 3D pane and a
dedicated top-down pane (both Spatial3DView over origin="world", named
"3D" and "top_down" so dimos-viewer persists camera state separately).
demo_better_pgo_viz.py composes the cross-wall sim blueprint with
agentic_debug=True so the new layout + pose graph render together. Used
for manual screenshot validation.
Adds visual_override entries for world/pgo_graph_nodes and world/pgo_graph_edges that mirror the existing FAR pattern: when agentic_debug=True, the PGO pose graph renders at z=_AGENTIC_DEBUG_LIFT (3.0m) instead of the default 1.7m, with slightly larger node radii (0.15) and edge thickness (0.06) so the green keyframe trajectory stands out clearly above the terrain cloud in the top-down pane. Verified visually via demo_better_pgo_viz with the cross-wall sim — green keyframe nodes + edges are now plainly identifiable above terrain in both the 3D and top_down rerun panels.
rerun's Spatial3DView doesn't have a top-down camera API, so the "top_down" pane introduced in a7a9be9 was just a duplicate 3D view. Drop _agentic_debug_rerun_blueprint and use _default_rerun_blueprint unconditionally — the agentic_debug lift on visual_override is what actually makes the pose graph and nav markers readable from any angle.
C++ side (main.cpp): when searchForLoopPairs sets m_cache_pairs (i.e. this keyframe will be incorporated into iSAM2 with a loop factor), snapshot the current global poses before smoothAndUpdate. After the update, build a nav_msgs::Path-encoded LoopClosureDeltas message: position = post.t - r_delta * pre.t, orientation = quaternion(post.R * pre.R^T). Publish on the new pgo_loop_closure topic. Stderr logs the event count for live observability. Python side (pgo.py): declare pgo_loop_closure: Out[NavPath] so the new topic is registered alongside corrected_odometry/pgo_tf/etc. Slow test (test_pgo_loop_closure.py): replays og_nav_60s through the native binary with permissive thresholds (loop_time_thresh=5s, min_loop_detect_duration=1s, loop_search_radius=2m, loop_score_thresh=0.5) so the recording reliably triggers loop closures. Subscribes to pgo_loop_closure, logs each event the moment it arrives (event #, poses_length, frame_id, first delta), and after the run validates each event has >0 poses, finite translations (<100m), and unit-norm quaternions (drift <0.05). Stdout from a run shows 19 events, sizes 10..35, max |t|=0.0013m, max |q|-1|=1e-6 — exactly the small-nudge profile expected from a self-consistent recording.
Replaces the kdtree-on-keyframe-positions loop search with a Scan
Context (Kim & Kim 2018) descriptor-based pipeline:
1. addKeyPose now also caches a polar-binned (20 rings × 60 sectors)
max-z descriptor + the per-row mean "ring key" for each keyframe.
The descriptor is appearance-based and pose-independent, so it
keeps working even when odometry has drifted enough that the new
keyframe is no longer "near" its old neighbours in pose-space.
2. searchForLoopPairs first asks Scan Context for a candidate:
ring-key L2 distance ranks all past keyframes, top-K are scored
by column-shifted cosine distance on the full descriptor, the
best below the threshold (default 0.4) is the candidate. The
winning column shift is also converted to a yaw rotation and used
to seed ICP, which dramatically improves convergence on revisits
that arrive at a different heading from the original pass.
3. Position-based search is retained as a fallback when SC is
disabled or finds nothing, so existing behaviour is preserved.
Replaces ~50 lines of position-search with ~30 lines of SC retrieval
in searchForLoopPairs; new scan_context.{h,cpp} (~150 lines, MIT
attribution to upstream irapkaist/scancontext concepts but no source
copied) implements the descriptor + distance.
Side-effect: this makes on-start relocalization a small follow-up
addition — descriptors + ring-keys + poses are now per-keyframe state
that can be serialised, and the SC search path already does
"appearance-based pose recovery without an initial pose guess."
Verified via test_pgo_loop_closure.py: 17 loop-closure events fired
across the og_nav_60s rosbag (was 19 with naive position search; SC
is more selective and rejects two borderline-position matches that
weren't actually visual revisits). All events have valid shape + tiny
quaternion/translation deltas as expected for a self-consistent bag.
…n search misses Adds CLI args to expose Scan Context config on the native binary (--use_scan_context, --sc_n_rings, --sc_n_sectors, --sc_max_range_m, --sc_top_k, --sc_match_threshold). New slow test test_pgo_synthetic_drift.py: - Synthesises a 4-wall point-cloud room with two distinctive interior columns (so the scene isn't rotationally symmetric). - Generates an out-and-back trajectory: drives east 8m then returns to the origin, heading unchanged. - Injects DRIFT_AT_REVISIT_M = 5m of additive y-drift into the reported odometry, ramped linearly with travelled distance. The body-frame scan stays byte-identical between first and second visit (same true sensor view of the same scene); the odom pose at revisit is 5m offset. - Runs the native PGO binary twice over the same input: * use_scan_context=true → expect ≥1 loop event * use_scan_context=false → expect 0 loop events (drift >> 1m radius) - Dumps PGO stderr after each run for diagnostics. Result: SC fires 10 loop closure events on the synthetic trajectory; position-based search fires 0 — exactly the demonstration of why we swapped to appearance-based place recognition. Both assertions pass. Verifies the core SC value prop: appearance-based place recognition doesn't depend on the (drifted) pose, so it keeps working when the odometry has wandered far enough that the kdtree-on-positions search no longer finds neighbours.
Test files now use setup_logger() / logger.info(...) per the fix_nits rule "no print() calls in tests; use logging if diagnostics are genuinely needed." Matches the existing test_pgo_rosbag.py convention. Also drops the now-unused sys import. Also clears a stale docstring on demo_better_pgo_viz.py: it claimed the demo enabled a "horizontal 3D + top-down panes" layout, which was reverted in 1801759 — rerun's Spatial3DView didn't support an initial camera angle (rrb.EyeControls3D existed at the time but wasn't used). The remaining value of agentic_debug=True is the visual override lift, which the new docstring describes accurately. No behavioural change. Tests still pass.
Sweep over names introduced by the better_pgo work that hit fix_nits
"expand mod -> module" rule:
- scan_context: cfg -> config (param + 12 call-sites); d (return val) ->
descriptor in make_descriptor/make_ring_key/make_sector_key; pt -> point
in the descriptor build loop; zf -> point_z (float cast); q_col/c_col
-> query_column/candidate_column; q_norm/c_norm -> query_norm/
candidate_norm; cj -> shifted_j; d (in best_distance return loop) ->
distance with min_distance for the running best.
- simple_pgo: desc -> descriptor on the per-keyframe cache; k ->
top_k_count for the partial-sort bound; structured-binding `auto [d,
shift]` -> `auto [distance, shift]`.
- main.cpp: kp -> keyframe; ps -> pose_stamped (build_graph_nodes and
build_loop_closure_deltas); a/b -> start/end and p1/p2 ->
start_pose/end_pose in append_segment; n -> count for the loop bound;
lc_msg -> loop_closure_msg at the publish site.
- tests: ps -> pose in the validate loop (test_pgo_loop_closure);
c,s -> cos_yaw,sin_yaw in _yaw_rotation (test_pgo_synthetic_drift).
Names that intentionally stay short are the math-convention ones:
r/t for SE(3) rotation+translation, q for quaternion, i/j as loop
indices, idx as keyframe index, ts as timestamp, dt for time delta,
tx/ty/tz/qx/qy/qz/qw for component decomposition. The fix_nits rule
calls out mod/lc as the target pattern; expanding the math-notation
names would make the code less readable, not more.
Also drops one section-label comment ("# Log each event the moment it
arrives.") whose adjacent function name already conveys the same and
one in-loop "# node_type 1 = odom/robot" that repeats info already
stated in the function-level docstring.
Native binary rebuilt + slow test still passes (17 events, all valid).
Drops in the wiring for evaluating the PGO native module on KITTI-360. Cannot run end-to-end yet — the dataset is gated behind a registered login at cvlibs.net so the data download is a manual user step. What's here: - kitti360_loader.py: parses the KITTI-360 directory layout (data_3d_raw + data_poses + calibration); composes per-frame lidar→world pose by chaining cam0_to_world ⊕ inv(velo_to_cam). Exposes a frame iterator + scan_xyz(frame_id). - loop_groundtruth.py: LCDNet/KITTI-convention groundtruth (≥50 frame gap, ≤4m radius), order-agnostic scoring of detected pairs. - run_kitti360_benchmark.py: argparse CLI, spawns the native binary on private LCM topics, plays (registered_scan, odometry) from disk, subscribes to pgo_graph_edges to extract loop-closure pairs (via traversability ≈ 0.4 segments) and pgo_loop_closure for delta event counts. Writes JSON. - README.md: download instructions for the official "Test SLAM 3D" 12 GB package, published SOTA reference numbers from LCDNet + ISC papers (LCDNet 0.91-0.93 AP, Scan Context 0.62-0.78 AP), expected ballpark for our minimal SC port.
Codecov Report❌ Patch coverage is 📢 Thoughts on this report? Let us know! |
Contributor
Greptile SummaryThis PR wires PGO to publish pose-graph nodes, edges, and loop-closure delta events as
Confidence Score: 4/5Safe to merge modulo the lingering SC→ICP miss discussed in the prior review round; no new blocking issues were introduced. The large batch of fixes from previous review rounds are all correctly implemented — port types, metric dimensions, playback error guards, timestamp cache alignment, and SC config exposure. The one structural gap still in simple_pgo.cpp (a Scan Context candidate that fails ICP silently exits without trying the positional fallback) was already called out and is not new to this iteration. No new issues at that level were introduced by this PR. dimos/navigation/nav_stack/modules/pgo/cpp/simple_pgo.cpp — the SC false-positive → missed position-fallback path is still present and worth a follow-up. Important Files Changed
Sequence DiagramsequenceDiagram
participant KP as Kitti360PlaybackModule
participant PGO as PGO (C++ binary)
participant SC as PoseGraphScoringModule
KP->>PGO: odometry (send_timestamps[i])
KP->>PGO: registered_scan (send_timestamps[i])
PGO-->>KP: corrected_odometry (startup gate)
loop every keyframe
PGO->>PGO: searchByScanContext / searchByPosition
PGO->>PGO: ICP alignment
PGO->>SC: "pose_graph_edges (NavPath, loop edges w/ traversability=0.4)"
PGO->>SC: loop_closure (NavPath, per-keyframe SE3 deltas)
PGO->>KP: corrected_odometry
end
note over SC: _on_pose_graph_edges: timestamp to frame_id lookup, query-level TP/FP/FN counting
KP->>KP: "_playback_finished = True (finally)"
SC->>SC: get_results() → precision / recall / F1
Reviews (30): Last reviewed commit: "fixup on macos" | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
- run_kitti360_benchmark: type the scipy Rotation.as_quat result to silence no-any-return. - demo_better_pgo_viz: annotate build_blueprint() -> Blueprint.
shift comes from best_distance, which scans [0, n_sectors-1], so the raw yaw is in (-2pi, 0] and `yaw > M_PI` can never fire. Only the negative-wrap guard is needed to normalize into [-pi, pi].
dimos/ disallows __init__.py files (test_no_init_files) — the empty one in pgo/benchmark/ slipped in with c7fd631 and was tripping the 3.14 test job.
mypy can't infer parameter types on a lambda subscribed to LCM; lift the body into a tiny factory function with explicit Callable[[str, bytes], None] signature so the lint job passes.
sklearn doesn't ship a py.typed marker; the new place_recognition_ap benchmark is the only sklearn user in dimos, so a per-import ignore is the smallest fix to unstick the lint job.
Naming sweep across dimos/navigation/nav_stack/modules/pgo per fix_nits review conventions: expanded short locals (frac, true_pos, dt, ts, pos, yaw, msg, sub, idx, fid, gt, cfg, desc, dots, sims, dists, pa/pb, …) to full descriptive names in the benchmark scripts, the synthetic-drift test, and the loop-closure test. Greptile P1 fixes on PR #2099: - c2: benchmark sender and the timestamp_ms→frame_id cache now share a single _compute_send_timestamps source of truth. Previously the cache was keyed by raw KITTI timestamps while the runner sent max(raw_ts, 1.0 + index*0.001), so early-frame endpoints in PGO loop edges never matched the cache and were silently dropped — deflating recall without any warning. - c3: load_kitti360_sequence now raises on .bin/timestamps.txt length mismatch instead of silently leaving timestamps={}, which previously caused the benchmark to report recall=0 with no indication that the timestamp file was unusable. - c1 follow-up: rename the misspelled C++ field loop_score_tresh → loop_score_thresh in simple_pgo.{h,cpp} and main.cpp. The CLI flag was always spelled correctly; this is cosmetic but removes the source of confusion greptile (correctly) flagged. New regression test (test_pgo_synthetic_drift.py): test_scan_context_catches_reverse_loop drives the synthetic robot out 8m facing east, turns 180°, and drives back facing west. Body-frame scans on the return leg are rotated 180° relative to outbound, so this exercises the init_guess fix in searchForLoopPairs (yaw rotated about the source keyframe instead of the world origin). Reverting that fix reproduces the failure. New runners: - benchmark/place_recognition_ap.py: apples-to-apples place-recognition AP eval against published Scan Context numbers. Reports AP=0.97-0.98 on seq 02/04/08 of the Test SLAM split, with precision 1.000 at the Kim & Kim 0.13 threshold. - benchmark/smoke_test.py: ~10s liveness probe that subscribes to all six PGO output topics, captures stderr, and prints per-topic message counts plus a one-line verdict — used to distinguish "PGO crashed" vs "no keyframes" vs "no loops" vs "alive" during debugging. The benchmark runner also now captures PGO's stderr and dumps it behind --print-stderr; previously its diagnostic prints (keyframes, loop-closure events) were discarded.
jeff-hykin
changed the title
These tests build and run the C++ PGO binary via `nix build .#default`. The default ubuntu CI runners don't have nix (or git-lfs), so the build aborts with `exit 127`, the PGO module never starts, and the test scaffolding leaks `_lcm_loop` host threads while unwinding the half-built state. `slow` wasn't filtering them out — only `tool`, `self_hosted`, `mujoco`, and `dimsim` are deselected by the default pytest addopts. `self_hosted` matches what test_pgo_rosbag.py already uses.
The self-hosted Linux runner (`ghcr.io/dimensionalos/ros-dev:dev`)
doesn't have nix installed, so PGO.build() exits 127 immediately
("`/bin/sh: 1: nix: not found`"). The four PGO tests that drive the
C++ binary need nix to compile it; mark them `skipif_no_nix` in
addition to `self_hosted` so they skip on the Linux self-hosted
container and still run on the macos self-hosted runner (which has
nix).
- dimos/conftest.py: register `skipif_no_nix` marker (uses
`shutil.which("nix")`), parallel to `skipif_no_ros`.
- Mark test_pgo_synthetic_drift, test_pgo_loop_closure,
test_pgo_rosbag with `skipif_no_nix`.
Two new greptile P1s on the KITTI-360 benchmark. scoring.py / loop_groundtruth.py: _score_pairs counted TP per edge and FN per query, so recall = (edge TP) / (edge TP + query FN) — a mixed denominator that inflates the metric. With 4 correct edges per query at the default search radius, recall reads ~0.96 instead of the true per-query 0.90. Make TP query-level too: a query contributes 1 TP if *any* of its edges matched. Precision now correctly tracks edge-level false positives against query-level true positives, matching the docstring's "a single correct detection per query is enough" intent. playback.py: _run_playback failed to set _playback_finished if any frame raised (e.g. partial KITTI-360 download missing a `.bin`). The exception silently terminated the asyncio task and runner.py's `while not playback.is_finished()` poll would spin forever, never reaching `finally: coordinator.stop()` — leaking the coordinator and all worker processes. Wrap the loop body in try/except/finally, stash the error string in `_playback_error` (new @rpc), and finally-set `_playback_finished = True` so callers always exit the poll. runner.py + benchmark_kitti_smoke_test.py: check playback_error() once the poll exits; if set, raise RuntimeError so the caller sees the real cause instead of just `dict.get('precision') is None`.
Same `try/except/finally` pattern that was added to Kitti360PlaybackModule in ed64f25 — if any statement inside the playback loop raises, `_playback_finished` now flips to True via `finally` so callers polling `is_finished()` can exit, and the exception string is stashed in `_playback_error` (new @rpc). Otherwise a missing scan would deadlock test_pgo_rosbag's poll loop and leak the coordinator.
Last of the three playback modules to get the try/except/finally treatment. If any publish raises mid-loop, `_playback_finished` would have stayed False forever, and `_run_pgo`'s `while not playback.is_finished()` poll loop would hang the test and leak the coordinator. Now finally-set, plus a new `playback_error()` @rpc exposes the exception string to callers.
The previous fix moved TP to query-level but left FP at edge-level, so `precision = TP / (TP + FP)` was still mixing dimensions. Switch FP to a `seen_queries_without_hit` set and subtract the TP-set so a query that fires *any* correct edge collapses to a single TP (mixed detections aren't punished). Both _score_pairs (scoring.py) and score_detected_loops (loop_groundtruth.py) updated identically.
…oke.py The file is a standalone manual script with argparse + print() output, not a pytest test. Per CLAUDE.local.md convention, manual scripts get a `demo_` prefix so pytest skips them and the `print()` usage is clearly intentional (rule 2 only applies to actual pytest tests). No internal references to update.
The rename from benchmark_kitti_smoke_test.py to demo_benchmark_kitti_smoke.py let the auto-registration scanner pick up the inline TopicCounterModule (test files are excluded, demo files aren't). Regen so CI's test_all_blueprints_is_current passes.
* New ``dimos/navigation/nav_stack/specs.py`` with a ``LoopClosure``
Protocol that declares the in/out streams a loop-closure-producing
pose-graph SLAM module must expose:
- registered_scan: In[PointCloud2]
- odometry: In[Odometry]
- loop_closure: Out[NavPath]
- pose_graph_edges: Out[NavPath]
* ``run_benchmark`` annotates ``module_under_test`` as ``LoopClosure``
instead of bare ``Blueprint`` so the protocol the caller has to
satisfy is visible in the signature.
* Playback module docstring now references the spec rather than
describing the stream contract inline.
* Scoring keeps ``loop_closure_traversability`` and
``traversability_tolerance`` as config fields (not RPC-discovered);
the spec only constrains the stream shape, the tag value remains
a per-blueprint configuration knob.
Comment on lines
+246
to
269
| // source keyframe's own global position instead: | ||
| // init = T(source_position) · Rz(yaw) · T(-source_position) | ||
| // → init · p = rotation · (p - source_position) + source_position | ||
| Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity(); | ||
| if (m_config.use_scan_context && sector_shift != 0) { | ||
| const double yaw = scan_context::yaw_from_shift(sector_shift, m_scan_context_config.n_sectors); | ||
| Eigen::AngleAxisf yaw_axis_angle( | ||
| static_cast<float>(yaw), Eigen::Vector3f::UnitZ()); | ||
| Eigen::Matrix3f rotation = yaw_axis_angle.toRotationMatrix(); | ||
| Eigen::Vector3f source_position = m_key_poses[cur_idx].t_global.cast<float>(); | ||
| init_guess.block<3, 3>(0, 0) = rotation; | ||
| init_guess.block<3, 1>(0, 3) = source_position - rotation * source_position; | ||
| } | ||
|
|
||
| CloudType::Ptr target_cloud = getSubMap(loop_idx, m_config.loop_submap_half_range, m_config.submap_resolution); | ||
| CloudType::Ptr source_cloud = getSubMap(m_key_poses.size() - 1, 0, m_config.submap_resolution); | ||
| CloudType::Ptr source_cloud = getSubMap(cur_idx, 0, m_config.submap_resolution); | ||
| CloudType::Ptr align_cloud(new CloudType); | ||
|
|
||
| m_icp.setInputSource(source_cloud); | ||
| m_icp.setInputTarget(target_cloud); | ||
| m_icp.align(*align_cloud); | ||
| m_icp.align(*align_cloud, init_guess); | ||
|
|
||
| if (!m_icp.hasConverged() || m_icp.getFitnessScore() > m_config.loop_score_tresh) | ||
| if (!m_icp.hasConverged() || m_icp.getFitnessScore() > m_config.loop_score_thresh) | ||
| return; |
Contributor
There was a problem hiding this comment.
When searchByScanContext returns a candidate (loop_idx >= 0) but ICP subsequently rejects it (poor fitness or divergence), searchForLoopPairs hits the return on line 268 and exits — searchByPosition is never attempted. The position fallback is guarded only by loop_idx < 0 before ICP, not after ICP rejects the SC candidate.
Concrete failure: SC picks a geometrically similar but spatially distant keyframe, ICP fails to converge on the mismatched sub-map, and a valid spatially-close loop (within loop_search_radius) that searchByPosition would have found is silently skipped. Loop drift accumulates without correction and no diagnostic is logged. This is a recall regression vs. use_scan_context=false.
Fix: save the SC loop_idx aside, attempt ICP, and on failure set loop_idx = searchByPosition() before checking convergence again — or restructure so both candidates are scored and the better one is kept.
NativeModule.start() previously returned the moment Popen launched the
binary; the C++ side still needed ~100ms-2s to actually call
lcm.subscribe() on its input topics, and any publisher wired to PGO
during that window lost messages.
Now:
* PGO declares ``ready: Out[String]``.
* PGO.start() generates a fresh nonce, subscribes to the ready
channel, then publishes ``python:{nonce}`` every 100ms until it
receives ``native:{nonce}`` (timeout 30s).
* The C++ binary's ``main`` subscribes to all input topics first,
then subscribes to ``ready`` with a ``ReadyHandshake`` handler
that echoes back ``native:{nonce}`` for any ``python:{nonce}``
message. By the time it sends the ack the binary is already
inside its handler-dispatch loop with all data subscriptions
active.
Also adds the standard ``dimos/msgs/std_msgs/String.py`` wrapper
around ``dimos_lcm.std_msgs.String``.
…cribed" This reverts commit e0fecd7.
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.
Closes #2007
Changes
How to Test
without benchmark
With benchmark
The benchmark is KITTI 360 - a standard dataset but its stuck behind a registration page (no public download). Its also 12Gb. https://www.cvlibs.net/datasets/kitti-360/user_login.php
There's no good 100% public loop closure benchmark that I could find.
Not in this PR
Contributor License Agreement