gsc_pgo: online and offline PGO #2587
Conversation
Convert the memory2 Recorder from thread/disposable rx subscriptions to manual async callbacks via process_observable, and let pose_setter_for methods be async (awaited in _resolve_pose). Update the fastlio and go2 recorders accordingly.
Raise TypeError at decoration time if a non-async function is decorated, and always await the setter in _resolve_pose.
…imos into jeff/fix/pose_setter_for
…kitti, voxel_map, module_loading)
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 SummaryThis PR adds online and offline Pose Graph Optimization (PGO) for the Unitree Go2 robot — a Rust implementation (
Confidence Score: 4/5Safe to merge for the online PGO flow; two resource and correctness gaps in the offline tooling need attention before heavy offline use. The Rust PGO core and eval harness are well-structured. Two concrete defects exist in the offline pipeline: Files Needing Attention: Important Files Changed
Sequence DiagramsequenceDiagram
participant DB as Recording DB
participant Replay as LockstepReplay
participant PGO as GscPGO (Rust)
participant Capture as GraphCapture
participant Eval as evaluate()
Eval->>DB: count lidar/odom rows
Eval->>Replay: start (db, streams, strides)
loop For each odom message
Replay->>PGO: Odometry (fire-and-forget, paced)
end
loop For each lidar scan
Replay->>PGO: PointCloud2
PGO-->>Replay: corrected_odometry (ack)
PGO->>Capture: pose_graph (Graph3D)
opt loop detected
PGO->>Capture: loop_closure_event (GraphDelta3D)
end
end
Capture->>DB: graph JSON (via temp file)
Eval->>DB: read graph JSON
Eval->>Eval: drift_delta_lookup(graph, raw_odom)
Eval->>Eval: score_tags + lidar_voxel_agreement
Eval->>Eval: write topdown PNG / isometric PNG / RRD
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
…eff/feat/jnav_pgo
…ose cached stores
…ms) so add_april imports resolve
jeff-hykin
changed the title
The gsc_pgo/rust crate links C++ gtsam via an FFI build script and cannot compile without gtsam present, so the lint job's `pre-commit --all-files` run failed at cargo-clippy. The dedicated rust CI job already excludes this crate for the same reason; mirror that exclusion in the pre-commit hook so plain environments (and the lint job) pass. Clippy it locally in the nix flake dev shell.
Both LockstepReplay._load and RateReplay._load called store.stop() only on the happy path, so a deserialization or schema error inside either islice loop propagated out with the store's file handles still open. Wrap the iteration in try/finally so stop() always runs — same pattern as registered_scans in utils.py.
Recordings captured without a CameraInfo stream have no intrinsics for AprilTag post-processing. This script writes the Go2 static front-camera CameraInfo (K + distortion, camera_optical frame) into the recording db, adding the camera_info stream only if it is missing.
Add a camera_info CameraInfo stream to the china_office.db LFS recording so the AprilTag PGO stage runs without a fabricated intrinsics source. Simplify read_camera_info to the exact camera_info stream (no splat-search), un-gate rrd output, and add an empty-world-frame odom fallback for the legacy go2 path.
Iterating an untyped store.stream() left `observation` un-annotated, which mypy (no gsc_pgo scripts exemption) flags as var-annotated. Type it Any — only .ts is read.
…sing The post_process CameraInfo-missing warning now tells go2 users to run scripts/add_camera_info.py to inject the static front-camera intrinsics before re-running, instead of silently skipping the AprilTag stage.
github-actions
Bot
added
the
ready-to-merge
gsc_pgo vs IvanPGO self-consistency across every usable mid360/go2 recording, one config per sensor. Documents huge-loop closure (pointlio), go2/L1 harm cases, and the sf_office_survey zenoh-wedge skip.
…d loops The 80 m ICP gate skipped the huge-loop revisit when FAST-LIO odometry had drifted ~60-175 m by the time the robot returned, so the loop never closed (corrected start-end spread stuck at 12.9 m). Raising the default to 200 m lets the far-drifted revisit reach ICP: fastlio huge loop 59.6 -> 2.3 m, pointlio 10.8 -> 0.09 m, with no previously-good mid360 map regressing. go2 is unaffected (its blueprint overrides this to 13.42 m).
Raising loop_candidate_max_distance_m 80->200 closes the FAST-LIO huge loop (59.557 -> 2.275 m) and tightens pointlio (0.329 -> 0.093 m); no previously-good mid360 row regresses to HARM. Also benches the two sf_office_survey go2 recordings (survey1 no-harm, survey2 marginal HARM) that were previously skipped.
github-actions
Bot
added
ready-to-merge
Use the store's context-manager protocol (with SqliteStore(...) as store) so stop() runs on early SystemExit, exceptions, and GeneratorExit. Replaces the leaking start()/stop() pairs in evaluate() and load_tag_detections(), and collapses the existing try/finally sites (registered_scans, replay _load).
github-actions
Bot
removed
the
ready-to-merge
github-actions
Bot
added
the
ready-to-merge
Wrap main() in `with SqliteStore(...) as store:` so the store's file handle and background thread are released on every exit path (greptile P1: previously only stop()'d on the happy path). Guard the corrected raycast_accumulate with `corrected_store_tf is not None` so --no-odom + --accum warns and skips instead of silently writing an empty cloud.
github-actions
Bot
removed
the
ready-to-merge
post_process.py already imports this to place AprilTag detections for recordings that carry a camera_info stream but no tf tree; the import was committed without its definition, leaving mypy/lint red.
github-actions
Bot
added
the
ready-to-merge
github-actions
Bot
removed
the
ready-to-merge
| lockstep: bool = True, | ||
| drift_per_sec: list[float] | None = None, | ||
| drift_t0: float = 0.0, | ||
| ) -> tuple[list[GraphPose], int, dict[str, Any]]: | ||
| """Replay the recording through the module; return its optimized pose graph | ||
| (with orientations), loop-closure count, and replay stats. | ||
|
|
There was a problem hiding this comment.
drift_t0 default
run_module_graph defaults drift_t0=0.0, so when a caller passes drift_per_sec=[dx, dy, dz] the computed per-message offset is drift * (unix_timestamp - 0.0) ≈ drift * 1.7e9 s. A "1 cm/s" injection immediately produces a ~17 000 km offset rather than a small perturbation from the start of the recording. The feature is documented and wired up, but its default value makes it effectively unusable without explicitly supplying a recording-start timestamp.
The fix is to auto-detect drift_t0 from the first message timestamp inside run_module_graph when drift is non-zero, e.g. drift_t0 = messages[0][0] after the load call.
| output_path = Path(tempfile.gettempdir()) / f"jnav_lc_eval_{db_path.parent.name}.json" | ||
| output_path.unlink(missing_ok=True) | ||
| done_path = Path(tempfile.gettempdir()) / f"jnav_lc_eval_done_{db_path.parent.name}.json" | ||
| done_path.unlink(missing_ok=True) | ||
| Path(str(done_path) + ".progress").unlink(missing_ok=True) |
There was a problem hiding this comment.
output_path and done_path are keyed by db_path.parent.name (e.g., "2026-06-04_12-56pm-PST"). Two parallel calls to run_module_graph for any two recordings that share the same parent directory name (a common situation in a batch benchmark runner that processes multiple mem2.db files from same-named sub-directories, or re-runs of the same recording) will overwrite each other's files. A late arrival from run A could feed stale graph data into run B's json.loads at the very end. Using tempfile.mkstemp or including more path components (e.g., db_path.stem + "_" + db_path.parent.name) in the name would remove the collision.
github-actions
Bot
added
the
ready-to-merge
# Conflicts: # dimos/robot/all_blueprints.py
| ) | ||
| cloud = o3d.geometry.PointCloud() | ||
| cloud.points = o3d.utility.Vector3dVector(points.astype(np.float64)) | ||
| if intensities is not None: | ||
| cloud.colors = o3d.utility.Vector3dVector( | ||
| np.repeat(intensities.astype(np.float64)[:, None], 3, axis=1) | ||
| ) | ||
| cloud, _keep = cloud.remove_statistical_outlier(LCM_OUTLIER_NN, LCM_OUTLIER_STD) | ||
| merged_xyz = np.asarray(cloud.points, np.float32) | ||
| merged_intensities = ( | ||
| np.asarray(cloud.colors, np.float32)[:, 0] if intensities is not None else None | ||
| ) | ||
| merged = PointCloud2.from_numpy( | ||
| merged_xyz, frame_id=world_frame, intensities=merged_intensities | ||
| ) | ||
| merged.ts = stamp | ||
| lcm_path.write_bytes(merged.lcm_encode()) | ||
| print( | ||
| f"wrote {lcm_path}: 1 aggregated cloud, {len(merged_xyz):,} pts (voxel {voxel} m)", | ||
| flush=True, | ||
| ) | ||
|
|
||
|
|
||
| def raycast_accumulate( | ||
| store: Store, |
There was a problem hiding this comment.
store_tf is None
raycast_accumulate accepts store_tf: RecordingTF | None, but when it is None the inner loop's continue skips every scan, leaving mapper empty and last_ts = 0.0. The function then writes PointCloud2.from_numpy([], ..., timestamp=0.0) into <in_stream>_accumulated — a silently wrong empty result at epoch-zero timestamp.
The caller in post_process.py correctly guards the corrected-lidar call (if corrected_store_tf is not None:) but does NOT guard the first call for the raw lidar stream. Passing --no-tf (which sets store_tf = None) combined with --accum therefore silently produces an empty raw-accumulated cloud while iterating the entire lidar stream for nothing. An early-return guard at the top of raycast_accumulate when store_tf is None (or a matching guard at the call site in post_process.py) would surface this as a clear diagnostic.
3 participants
Run real time
dimos run unitree-go2-mid360-pgo dimos run unitree-go2-pgo # very conservativeOffline PGO (uses april tags for correction if available)
Online PGO replay
Note: offline does better than online, but online is so good it doesn't really matter for the images
Online PGO: Mid360 Huge Loop Fastlio
Online PGO: Mid360 Huge Loop Pointlio
Online PGO: Mid360 China Office
Online PGO: Mid360 Gir Stairs