[jeff-hykin]

dimos run unitree-go2-mid360-pgo
dimos run unitree-go2-pgo # very conservative

Offline PGO (uses april tags for correction if available)

# china_office.db is pulled from LFS automatically (bare filename -> LFS), so this runs first-try.
python dimos/navigation/jnav/components/loop_closure/gsc_pgo/scripts/post_process.py \
    --db china_office.db \
    --odom pointlio_odometry \
    --lidar pointlio_lidar \
    --tags raw_april_tags \
    --camera color_image \
    --tag-size 0.10 \
    --dict DICT_APRILTAG_36h11 \
    --world-frame world \
    --corrected-odom-frame corrected_odom \
    --corrected-suffix _corrected \
    --suffix "" \
    --closure-spacing 2.0 \
    --lcm-voxel 0.05 \
    --accum-voxel 0.05 \
    --accum-max-range 20.0
# Graceful on other recordings: --odom-tf auto-resolves from the odom stream's own header,
# no camera_info -> AprilTag stage is skipped (ICP + odom only), and a missing base<-optical
# tf edge falls back to the known rig mount geometry. Pass --odom / --lidar to override the
# auto-detected stream names, or --base-optical 'x y z qx qy qz qw' for a non-Go2 rig.
# opt-out toggles (every stage is on by default):
#   --no-odom  --no-lidar  --no-icp  --no-lcm  --no-rrd  --no-accum  --no-tf

Online PGO replay

# eval generates a json and visuals (rrd, plots). china_office.db is pulled from LFS automatically.
uv run python dimos/navigation/jnav/components/loop_closure/eval.py \
    --db-path china_office.db \
    --odom-stream pointlio_odometry \
    --lidar-stream pointlio_lidar \
    --camera-stream color_image \
    --odom-tf odom:mid360_link \
    --tag-frame camera_optical \
    --module-path dimos/navigation/jnav/components/loop_closure/gsc_pgo/module.py \
    --module-name GscPGO \
    --pgo-config-json '{"use_scan_context": true}'
# --odom-tf is 'parent:child' for the odom edge (odom:mid360_link here). On another recording,
# set it to that db's odom parent:child; a missing camera_info stream skips the AprilTag stage.

Note: offline does better than online, but online is so good it doesn't really matter

Examples

[greptile-apps]

Greptile Summary

This PR introduces the gsc_pgo online and offline Pose Graph Optimization system: a Rust+GTSAM loop-closure module (Scan Context++ descriptors, ICP closures, GNC), an offline multi-stage post-processing pipeline, a lockstep replay evaluation harness, and a BaselinePGO Python wrapper around the existing C++ PGO state — all wired into the Go2 mid360 and L1 blueprints.

• Online PGO (gsc_pgo/module.py, Rust binary): GscPGO NativeModule publishes corrected odometry, a pose graph, and TF corrections; the lockstep replay harness feeds recordings through the module and captures the final graph for scoring.
• Offline PGO (post_process.py): Two-stage AprilTag PGO + ICP loop-closure solve; outputs corrected odom/lidar streams, deformation nodes, and aggregated .pc2.lcm clouds back into the recording db.
• Evaluation (eval.py, utils.py): Self-consistency scoring (tag-spread + lidar-voxel agreement) with before/after top-down/isometric/rrd visuals; RecordingTF caches the full tf stream with a past-only latch so near-static edges published once at recording start survive later lookups.

Confidence Score: 4/5

• The PR is mostly safe to merge; the one new defect found is isolated to the BaselinePGO pass-through path for placeholder Go2 poses and does not affect the primary GscPGO module.
• The Rust PGO core and the offline pipeline are large and novel, and the Rust crate has no CI coverage (builds only in the Nix flake dev shell). The BaselinePGO._publish_corrected function calls _transform_to_pose3 with a zero-quaternion odom on the Go2's startup placeholder poses, which produces an invalid GTSAM Pose3 and corrupts the published corrected odometry for those early frames. Everything else reviewed — GscPGO, RecordingTF, the eval harness, and the blueprints — looks correct for the intended use cases.
• baseline_pgo/module.py (_publish_corrected zero-rotation path); gsc_pgo/rust/ (no CI — must be validated in the Nix dev shell before any Rust changes)

Important Files Changed

Filename	Overview
dimos/navigation/jnav/components/loop_closure/baseline_pgo/module.py	New BaselinePGO module wrapping the existing C++ PGO state; contains a logic bug where a zero-rotation placeholder odom (from Go2 startup) is passed to _transform_to_pose3 inside _publish_corrected, producing a NaN GTSAM Pose3. Also accesses several private _PGOState fields throughout.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/module.py	New GscPGO NativeModule wiring the Rust PGO binary; port types match the binary's outputs. The _on_correction_for_tf subscription pattern correctly republishes the Rust-emitted TFMessage to the tf port. Config is detailed and well-documented.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/utils/replay.py	Lockstep replay harness and run_module_graph; temp-file paths keyed only on the parent directory name can collide in parallel batch runs. counts_store not protected by try/finally (also noted in previous review). drift_t0=0.0 default makes drift injection effectively unusable without an explicit start timestamp (also in previous review).
dimos/navigation/jnav/components/loop_closure/eval.py	Evaluation driver for loop-closure modules against a recording. SqliteStore lifecycle is now managed with a with statement, addressing the prior open-handle concern. The empty odom stream guard is present. Several edge cases around empty streams and drift injection were previously flagged and appear partially addressed in this version.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/scripts/post_process.py	Offline PGO pipeline (AprilTag PGO + ICP loop closures). SqliteStore is now managed via with. Empty odom guard present. Several issues previously flagged (empty lidar stream crash, LCM write crash on zero scans, corrected_store_tf=None accumulation) are partially addressed in the current version, though some edge cases remain.
dimos/navigation/jnav/components/loop_closure/gsc_pgo/utils/artifacts.py	Write helpers for corrected odom, lidar, deformation nodes, pose graph, and raycast-accumulated maps. write_aggregated_lcm crashes on an empty scan list (previously flagged). raycast_accumulate signature no longer accepts None for store_tf, addressing the silent-empty-cloud path from previous review.
dimos/navigation/jnav/utils/recording_tf.py	New RecordingTF / PastOnlyTBuffer that caches the full tf stream and overrides the odom edge. The lazy-load + past-only latch design correctly handles near-static frames published once at recording start. Edge override drops all time-varying recorded edges before injecting the fed trajectory.
.github/workflows/ci.yml	Excludes the gsc_pgo/rust crate from CI Cargo/Clippy (no gtsam on plain Ubuntu runner); acknowledged in comments as intentional — build and tests must be run locally in the Nix flake dev shell. No other structural CI changes.

_{Reviews (52): Last reviewed commit: "gsc_pgo: relock rust deps after merging ..." | Re-trigger Greptile}

[codecov]

Codecov Report

❌ Patch coverage is 37.74235% with 1445 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/apriltags.py	27.30%	367 Missing and 3 partials ⚠️
...s/navigation/jnav/components/loop_closure/utils.py	13.22%	269 Missing ⚠️
dimos/navigation/jnav/utils/trajectory_metrics.py	15.50%	158 Missing ⚠️
...omponents/loop_closure/gsc_pgo/scripts/make_rrd.py	19.55%	143 Missing and 1 partial ⚠️
...components/loop_closure/gsc_pgo/utils/artifacts.py	21.71%	137 Missing ⚠️
...mponents/loop_closure/gsc_pgo/utils/offline_pgo.py	42.22%	103 Missing and 1 partial ⚠️
dimos/navigation/jnav/msgs/Graph3D.py	35.92%	66 Missing ⚠️
dimos/navigation/jnav/utils/recording_tf.py	31.52%	63 Missing ⚠️
dimos/navigation/jnav/msgs/GraphDelta3D.py	34.17%	52 Missing ⚠️
dimos/navigation/jnav/msgs/DeformationNode.py	47.61%	22 Missing ⚠️
... and 8 more

@@            Coverage Diff             @@
##             main    #2587      +/-   ##
==========================================
- Coverage   77.83%   77.18%   -0.65%     
==========================================
  Files        1329     1377      +48     
  Lines      126868   131092    +4224     
  Branches    11035    12775    +1740     
==========================================
+ Hits        98743   101188    +2445     
- Misses      24974    26704    +1730     
- Partials     3151     3200      +49     

Flag	Coverage Δ
OS-ubuntu-24.04-arm	73.80% <37.74%> (-0.75%)	⬇️
OS-ubuntu-latest	74.32% <37.74%> (-0.76%)	⬇️
Py-3.10	74.32% <37.74%> (-0.76%)	⬇️
Py-3.11	74.31% <37.74%> (-0.76%)	⬇️
Py-3.12	74.32% <37.74%> (-0.76%)	⬇️
Py-3.13	74.32% <37.74%> (-0.76%)	⬇️
Py-3.14	74.32% <37.74%> (-0.76%)	⬇️
Py-3.14t	74.32% <37.74%> (-0.76%)	⬇️
SelfHosted-Large	30.53% <22.23%> (+0.33%)	⬆️
SelfHosted-Linux	35.02% <22.23%> (+0.37%)	⬆️

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

Files with missing lines	Coverage Δ
.../loop_closure/gsc_pgo/scripts/test_post_process.py	100.00% <100.00%> (ø)
...os/navigation/jnav/msgs/test_LocationConstraint.py	100.00% <100.00%> (ø)
dimos/navigation/jnav/utils/test_apriltags.py	100.00% <100.00%> (ø)
dimos/robot/all_blueprints.py	100.00% <ø> (ø)
...ree/go2/blueprints/smart/unitree_go2_mid360_pgo.py	90.90% <90.90%> (ø)
...ot/unitree/go2/blueprints/smart/unitree_go2_pgo.py	91.66% <91.66%> (ø)
.../robot/unitree/go2/go2_mid360_static_transforms.py	84.21% <33.33%> (-9.54%)	⬇️
...ion/jnav/components/loop_closure/gsc_pgo/module.py	94.50% <94.50%> (ø)
dimos/navigation/jnav/msgs/LocationConstraint.py	88.34% <88.34%> (ø)
dimos/navigation/jnav/utils/voxel_map.py	60.00% <60.00%> (ø)
... and 12 more

... and 80 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.

[TomCC7]

test

test