Skip to content

gsc_pgo: online and offline PGO #2587

Open
jeff-hykin wants to merge 24 commits into
mainfrom
jeff/feat/jnav_pgo
Open

gsc_pgo: online and offline PGO #2587
jeff-hykin wants to merge 24 commits into
mainfrom
jeff/feat/jnav_pgo

Conversation

@jeff-hykin

@jeff-hykin jeff-hykin commented Jun 24, 2026

Copy link
Copy Markdown
Member

gsc_pgo, Ports the PGO / loop-closure stack into the new jnav layout, plus a tf-tree feature for memory2 stores.

process_observable gains an optional on_drop callback fired once per
message dropped by the dispatcher's single-slot LATEST mailbox. The
Recorder uses it to count dropped frames per stream and log a throttled
warning, so a slow sink no longer loses data silently.
@greptile-apps

greptile-apps Bot commented Jun 24, 2026

Copy link
Copy Markdown
Contributor

Greptile Summary

This PR ports the PGO / loop-closure stack into the jnav layout, wiring a C++ GTSAM iSAM2 + PCL ICP binary as a NativeModule (gsc_pgo/module.py) and adding a full offline post-processing pipeline (AprilTag PGO → ICP refinement) together with a lockstep eval harness. It also adds a tf-tree feature to memory2 stores so recordings can answer world-registration lookups without baking in a fixed pose.

  • Online PGO module (gsc_pgo/module.py): wraps the C++ binary with typed In/Out ports, a Nix-pinned build command, and rich config covering Scan Context, tag loop closure, landmark factors, and anisotropic noise models; replaces the old CMU nav pgo registry entry.
  • Offline post-processing (post_process.py, detect_tags.py, add_april.py): two-stage solve (AprilTag PGO + ICP refinement) with per-glimpse quality gates, writes corrected odometry/lidar back to the recording db, and optionally emits a .pc2.lcm aggregate cloud.
  • memory2 tf feature (db_tf.py, store/base.py): lazy store.tf.get(target, source, ts) backed by the recorded tf/tf_static streams; double-checked locking with threading.Lock, table-name injection guarded, and public MultiTBuffer.lookup() used throughout.

Confidence Score: 5/5

Safe to merge — all previously-flagged concerns (SQL injection, private-method access, missing atexit cleanup, thread-safety) have been addressed in this revision.

The core additions (PGO NativeModule, offline post-processing pipeline, memory2 tf feature) are well-structured with proper table-name guards, double-checked locking, and a public lookup API. The eval harness adds a minor cross-class coupling and temp-file collision edge case, but neither affects production correctness — the eval scripts run in isolated dev sessions.

eval.py — temp-file naming and cross-class RateReplay dependency worth a follow-up cleanup; post_process.py — unconditional camera_intrinsics.json read should be deferred behind the RAW_STREAM guard for better UX on lidar-only recordings.

Important Files Changed

Filename Overview
dimos/navigation/jnav/components/loop_closure/gsc_pgo/module.py New PGO NativeModule wrapper; routes the C++ iSAM2+ICP binary, exposes well-typed In/Out ports, and publishes the correction as a TF transform on start. Config is comprehensive and well-documented.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/post_process.py Offline AprilTag+ICP post-processing script; SQL injection guarded via identifier regex, but camera_intrinsics.json is read unconditionally before the RAW_STREAM existence check.
dimos/memory2/db_tf.py New tf-tree overlay for Store; double-checked locking correct, uses public MultiTBuffer.lookup, table names validated with _safe_table.
dimos/navigation/jnav/components/loop_closure/eval.py Eval harness with lockstep ack-paced replay; cross-class call to RateReplay._payload_pose and temp-file paths keyed only by directory name are the two weak spots.
dimos/navigation/jnav/utils/recording_db.py Store cache now has close_all() and an atexit hook — previous resource-leak concern addressed.
dimos/memory2/store/base.py Adds lazy tf property to Store. Backward-compatible; no issues.
dimos/protocol/tf/tf.py Adds public lookup() method to MultiTBuffer suppressing per-miss log noise. Correct.
dimos/robot/all_blueprints.py Redirects 'pgo' registry key from CMU nav to gsc_pgo; adds eval harness entries. Intentional port.
dimos/core/module.py Adds optional on_drop callback to process_observable. Backward-compatible.
dimos/navigation/jnav/msgs/Landmark.py New Landmark message with backward-compatible LCM encode/decode and per-axis confidence fields.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/detect_tags.py Imports private _camera_speeds from apriltags.py creating a fragile undocumented dependency.

Sequence Diagram

%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
    participant LR as LockstepReplay
    participant PGO as gsc_pgo (C++ binary)
    participant GC as GraphCapture
    participant Eval as evaluate()

    Eval->>LR: start replay (odom + lidar)
    loop per scan
        LR->>PGO: odometry (fire-and-forget burst)
        LR->>PGO: lidar scan
        PGO-->>LR: corrected_odometry (ack)
        LR->>LR: advance to next scan
    end
    PGO-->>GC: pose_graph / loop_closure_event
    GC->>GC: write JSON to temp file
    Eval->>Eval: read JSON → score tag agreement + voxel agreement
Loading
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
    participant LR as LockstepReplay
    participant PGO as gsc_pgo (C++ binary)
    participant GC as GraphCapture
    participant Eval as evaluate()

    Eval->>LR: start replay (odom + lidar)
    loop per scan
        LR->>PGO: odometry (fire-and-forget burst)
        LR->>PGO: lidar scan
        PGO-->>LR: corrected_odometry (ack)
        LR->>LR: advance to next scan
    end
    PGO-->>GC: pose_graph / loop_closure_event
    GC->>GC: write JSON to temp file
    Eval->>Eval: read JSON → score tag agreement + voxel agreement
Loading

Reviews (6): Last reviewed commit: "rename DEFAULT_MARKER_LENGTH_M -> DEFAUL..." | Re-trigger Greptile

Comment thread dimos/memory2/db_tf.py
Comment thread dimos/memory2/db_tf.py Outdated
Comment thread dimos/navigation/jnav/utils/recording_db.py
Comment thread dimos/memory2/db_tf.py Outdated
Comment thread dimos/navigation/jnav/components/loop_closure/unrefined_pgo/module.py Outdated
@codecov

codecov Bot commented Jun 24, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 70.23810% with 175 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines Patch % Lines
dimos/navigation/jnav/utils/trajectory_metrics.py 15.67% 113 Missing ⚠️
dimos/navigation/jnav/utils/apriltag_agreement.py 57.35% 28 Missing and 1 partial ⚠️
dimos/navigation/jnav/utils/voxel_map.py 60.00% 12 Missing ⚠️
dimos/memory2/module.py 31.25% 10 Missing and 1 partial ⚠️
dimos/memory2/store/base.py 42.85% 4 Missing ⚠️
dimos/core/module.py 33.33% 1 Missing and 1 partial ⚠️
dimos/navigation/jnav/utils/kitti.py 95.00% 1 Missing and 1 partial ⚠️
dimos/protocol/tf/tf.py 50.00% 1 Missing ⚠️
.../robot/unitree/go2/blueprints/smart/unitree_go2.py 66.66% 1 Missing ⚠️
@@            Coverage Diff             @@
##             main    #2587      +/-   ##
==========================================
+ Coverage   69.61%   70.88%   +1.27%     
==========================================
  Files         878      876       -2     
  Lines       79326    77819    -1507     
  Branches     7126     6917     -209     
==========================================
- Hits        55220    55164      -56     
+ Misses      22301    20851    -1450     
+ Partials     1805     1804       -1     
Flag Coverage Δ
OS-ubuntu-24.04-arm 63.05% <70.23%> (+0.05%) ⬆️
OS-ubuntu-latest 65.86% <70.23%> (+0.03%) ⬆️
Py-3.10 65.85% <70.23%> (+0.03%) ⬆️
Py-3.11 65.86% <70.23%> (+0.04%) ⬆️
Py-3.12 65.85% <70.23%> (+0.03%) ⬆️
Py-3.13 65.85% <70.23%> (+0.03%) ⬆️
Py-3.14 65.87% <70.23%> (+0.03%) ⬆️
Py-3.14t 65.86% <70.23%> (+0.04%) ⬆️
SelfHosted-Large 30.09% <28.91%> (+<0.01%) ⬆️
SelfHosted-Linux 37.73% <28.91%> (-0.07%) ⬇️
SelfHosted-macOS 36.52% <28.91%> (-0.07%) ⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines Coverage Δ
dimos/navigation/jnav/msgs/Landmark.py 100.00% <100.00%> (ø)
dimos/navigation/jnav/msgs/Marker.py 100.00% <100.00%> (ø)
dimos/navigation/jnav/msgs/test_Landmark.py 100.00% <100.00%> (ø)
dimos/navigation/jnav/msgs/test_Marker.py 100.00% <100.00%> (ø)
...s/navigation/jnav/utils/test_apriltag_agreement.py 100.00% <100.00%> (ø)
dimos/navigation/jnav/utils/test_kitti.py 100.00% <100.00%> (ø)
dimos/robot/all_blueprints.py 100.00% <ø> (ø)
dimos/protocol/tf/tf.py 87.21% <50.00%> (-0.35%) ⬇️
.../robot/unitree/go2/blueprints/smart/unitree_go2.py 92.50% <66.66%> (ø)
dimos/core/module.py 77.29% <33.33%> (-0.33%) ⬇️
... and 6 more

... and 16 files with indirect coverage changes

🚀 New features to boost your workflow:
  • 📦 JS Bundle Analysis: Save yourself from yourself by tracking and limiting bundle sizes in JS merges.

@jeff-hykin jeff-hykin changed the title jnav: port PGO/loop-closure + tf-tree for memory2 stores gsc_pgo: online and offline PGO Jun 24, 2026
@jeff-hykin jeff-hykin enabled auto-merge (squash) June 24, 2026 08:35
@github-actions github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Jun 24, 2026
@github-actions github-actions Bot added ready-to-merge Required CI checks have passed on this PR and removed ready-to-merge Required CI checks have passed on this PR labels Jun 24, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

ready-to-merge Required CI checks have passed on this PR

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant