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)
jeff-hykin
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
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 ports the PGO / loop-closure stack into the new
Confidence Score: 4/5The core PGO module and eval harness are solid; the offline scripts have narrow crash paths under unusual recordings that do not affect the live robot stack. The .pc2.lcm aggregation path in post_process.py raises a bare ValueError whenever the lidar stream has zero scans. The make_rrd.py script retains unguarded key accesses on empty or diagnostics-free streams from prior review rounds. Neither issue affects the live unitree_go2_pgo blueprint at runtime. dimos/navigation/jnav/components/loop_closure/gsc_pgo/scripts/post_process.py and make_rrd.py — both contain crash paths for edge-case recordings that remain open from previous review rounds. Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant Rec as Recording DB
participant PP as post_process.py
participant GTSAM as GTSAM (LM)
participant ICP as Open3D ICP
participant Out as Output Streams
PP->>Rec: read raw_april_tags stream
PP->>Rec: SQL SELECT from odom stream
PP->>PP: select keyframes
PP->>GTSAM: Stage 1 - tag PGO
GTSAM-->>PP: corrected keyframe poses
PP->>Rec: read lidar submaps for candidate pairs
PP->>ICP: Stage 2 - ICP loop closures
ICP-->>PP: accepted BetweenFactorPose3 constraints
PP->>GTSAM: Stage 2 re-solve
GTSAM-->>PP: final estimate
PP->>Out: write gt_tf_deformation_nodes
PP->>Out: write pose_graph
PP->>Out: write gt_odometry
PP->>Out: write gt_lidar plus pc2.lcm
PP->>PP: build comparison RRD
%%{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 Rec as Recording DB
participant PP as post_process.py
participant GTSAM as GTSAM (LM)
participant ICP as Open3D ICP
participant Out as Output Streams
PP->>Rec: read raw_april_tags stream
PP->>Rec: SQL SELECT from odom stream
PP->>PP: select keyframes
PP->>GTSAM: Stage 1 - tag PGO
GTSAM-->>PP: corrected keyframe poses
PP->>Rec: read lidar submaps for candidate pairs
PP->>ICP: Stage 2 - ICP loop closures
ICP-->>PP: accepted BetweenFactorPose3 constraints
PP->>GTSAM: Stage 2 re-solve
GTSAM-->>PP: final estimate
PP->>Out: write gt_tf_deformation_nodes
PP->>Out: write pose_graph
PP->>Out: write gt_odometry
PP->>Out: write gt_lidar plus pc2.lcm
PP->>PP: build comparison RRD
Reviews (17): Last reviewed commit: "refactor(pointlio): remove dead config v..." | Re-trigger Greptile |
![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
…ory2 Replace the branch's memory2 tf-tree/DbTf layer with a jnav-local RecordingTF(StreamTF) built from the recording store, leaving memory2 net-zero vs origin/main (no changes to Ivan's code). RecordingTF full-loads the recording's tf once so one-shot static frames aren't evicted by StreamTF's windowed cache. post_process.py and eval.py now construct RecordingTF.from_store(store); the live-path deformation feature in protocol/tf is kept and its stale DbTfSql comment refs scrubbed.
|
|
||
| keyframe_indices = [0] | ||
| prev_rot, prev_pos = row_pose(odom_rows[0]) | ||
| for row_index in range(1, len(odom_rows)): |
There was a problem hiding this comment.
odom_rows[0] at line 301 throws IndexError if the SQL query returns zero rows (e.g. the stream table exists but is empty, or the recording was truncated). A recording with an empty odometry stream will produce an opaque traceback rather than a useful error. A guard with sys.exit before line 300 would surface the problem clearly.
Keep the PR focused on dimos/navigation/jnav. Revert protocol/tf, core, and lidar recorders to origin/main; drop the unused deformation buffer and DeformationNode consumer wiring; move DeformationNode into jnav (its only consumer is the gsc_pgo module). - revert dimos/protocol/tf/tf.py to main (lookup() had no callers) - delete dimos/protocol/tf/deformation.py + test (unused) - revert core/module.py on_drop plumbing (dead code) - revert fastlio2/pointlio recorders and go2 static transforms to main - move DeformationNode.py into dimos/navigation/jnav/msgs
| def accumulate(stream_name): | ||
| scans = [] | ||
| for scan_index, observation in enumerate(store.stream(stream_name)): | ||
| if scan_index % SCAN_STRIDE: | ||
| continue | ||
| points = np.asarray(observation.data.points_f32()) | ||
| if len(points): | ||
| scans.append(points[::3]) | ||
| all_points = np.concatenate(scans, 0) | ||
| _, unique_indices = np.unique( | ||
| np.floor(all_points / VOXEL).astype(np.int64), axis=0, return_index=True | ||
| ) | ||
| return all_points[unique_indices] |
There was a problem hiding this comment.
accumulate() crashes on empty or all-zero-point streams
np.concatenate(scans, 0) raises ValueError: need at least one array to concatenate when every scan in the stream is empty or the stream itself has no data. This path is reached unconditionally in build() for lidar_stream, so a recording with a sparse or missing lidar stream crashes the entire build() call — and by extension the OPEN_RRD branch of post_process.py — with an opaque NumPy traceback rather than a useful diagnostic. Adding a guard before the concatenate call (return an empty (0, 3) array or raise with a clear message) would make the failure legible.
github-actions
Bot
added
the
ready-to-merge
| @@ -0,0 +1,66 @@ | |||
| # Copyright 2026 Dimensional Inc. | |||
There was a problem hiding this comment.
needed something pure and lightweight for scoring (e.g. couldn't reuse existing voxel mapper)
detect_tags.py duplicated ensure_april_streams, which add_april.py already calls to build raw_april_tags (same gate diagnostics) plus the gated stream and summary. Repoint the two docstring/doc references to add_april.py.
Point the nix flake ref at github:jeff-hykin/gsc_pgo/v1.0.0 (a version branch at the previously-pinned rev) so the pin reads as a version and can be bumped on the gsc_pgo side without editing a hash here.
Scopes the "to" location variable to a named map so a constraint can close a loop across maps (cross-map closure). Empty = current/default map. Appended to the wire string group; roundtrip + defaults tests cover it.
github-actions
Bot
removed
the
ready-to-merge
| # world offset added at each time; the raw-baseline scoring must apply the | ||
| # SAME offset so it compares against what the module actually saw. | ||
| drift_per_sec = drift_per_sec or [0.0, 0.0, 0.0] | ||
| drift_t0 = next(iterate_stream(db_path, odom_stream))[0] if has_drift(drift_per_sec) else 0.0 |
There was a problem hiding this comment.
StopIteration crash on empty odometry stream when drift injection is enabled
next(iterate_stream(db_path, odom_stream)) at this line raises bare StopIteration when the odom stream exists in the database (passing the name-existence guard at line 671) but contains zero rows. The caller sees an opaque traceback pointing at line 682 — no indication that the stream is empty. Since drift injection is the primary use-case for drift_t0, this path is exercised by CI/eval runs that test drift correction, making this a real failure mode rather than a theoretical one.
…kind Supersedes the earlier single `map` string (added as a 4th field inside the existing string group) with the consolidated design from jeff/feat/repulsive-field-local-planner: map_id (multi-map frame system) and kind (coarse category, defaults to the to_id URL scheme) ride as tolerant TAIL fields after the covariance, so pre-merge payloads and the native gsc_pgo fixed-sequence decoder both stay compatible. Also adds from_confidence()/confidence and the (1-c)/c variance convention. Mirrors gsc_pgo v1.1.0 C++ decode (map_id + kind tolerant tail).
github-actions
Bot
added
ready-to-merge
The repo's test_no_section_markers convention check rejects the '# ----' banner comments; remove them (carried over from the source file).
| if not ( | ||
| float(tag_metrics["sharpness"]) >= GATE["s"] | ||
| and float(tag_metrics["reproj_px"]) <= GATE["r"] | ||
| and float(tag_metrics["tag_px"]) >= GATE["px"] | ||
| # older tag streams lack distance/view-angle; unknown passes the gate | ||
| and float(tag_metrics.get("distance_m", 0.0)) <= GATE["d"] | ||
| and float(tag_metrics.get("view_angle_deg", 0.0)) <= GATE["a"] | ||
| and ( | ||
| float(tag_metrics["lin_speed"]) < 0 | ||
| or float(tag_metrics["lin_speed"]) <= GATE["lv"] | ||
| ) | ||
| and ( | ||
| float(tag_metrics["ang_speed"]) < 0 | ||
| or float(tag_metrics["ang_speed"]) <= GATE["av"] | ||
| ) | ||
| ): |
There was a problem hiding this comment.
KeyError on lin_speed/ang_speed for tag streams without speed metadata
tag_metrics["lin_speed"] and tag_metrics["ang_speed"] use direct key access, but lin_speed and ang_speed are only written to a stream when _write_tag_stream(..., diagnostics=True). An older recording or a stream written without diagnostics won't carry these keys, causing KeyError and crashing build(). The immediately preceding lines already apply this pattern correctly for distance_m/view_angle_deg with .get() defaults — the same treatment is needed here. Use float(tag_metrics.get("lin_speed", -1.0)) (and likewise for ang_speed); a value of -1.0 always passes the gate because of the < 0 short-circuit already present in the condition.
Drop the local make_detector/_object_points/estimate_marker_pose/ reprojection_error_px in apriltags.py in favor of the shared create_aruco_detector/estimate_marker_pose/marker_reprojection_error in perception/fiducial/marker_pose.py.
github-actions
Bot
added
ready-to-merge
| odom_timestamps = np.array([timestamp for timestamp, _ in odom_poses]) | ||
| positions_by_marker = {} | ||
| for tag_observation in store.stream(tag_stream): | ||
| tag_metrics = tag_observation.tags | ||
| if not ( | ||
| float(tag_metrics["sharpness"]) >= GATE["s"] | ||
| and float(tag_metrics["reproj_px"]) <= GATE["r"] | ||
| and float(tag_metrics["tag_px"]) >= GATE["px"] | ||
| # older tag streams lack distance/view-angle; unknown passes the gate | ||
| and float(tag_metrics.get("distance_m", 0.0)) <= GATE["d"] | ||
| and float(tag_metrics.get("view_angle_deg", 0.0)) <= GATE["a"] | ||
| and ( | ||
| float(tag_metrics["lin_speed"]) < 0 | ||
| or float(tag_metrics["lin_speed"]) <= GATE["lv"] | ||
| ) | ||
| and ( | ||
| float(tag_metrics["ang_speed"]) < 0 | ||
| or float(tag_metrics["ang_speed"]) <= GATE["av"] | ||
| ) | ||
| ): | ||
| continue | ||
| tag_pose = tag_observation.data | ||
| closest_base_pose = odom_poses[ | ||
| int(np.argmin(np.abs(odom_timestamps - float(tag_observation.ts)))) |
There was a problem hiding this comment.
ValueError when the gt_odom stream exists but has zero rows
landmarks(gt_odoms[0]) is called only when gt_odoms is non-empty — meaning the stream name exists — but not when the stream has any rows. When store.stream(gt_odom) yields nothing, odom_poses = [] and odom_timestamps = np.array([]). The first tag observation that passes the gate then executes np.argmin(np.abs(np.array([]) - float(tag_observation.ts))), which raises ValueError: zero-size array to reduction operation fmin which has no identity, crashing the entire build() call. A recording in which post_process.py was interrupted after creating the gt_ stream entry but before writing any rows (e.g. a kill during the lidar-write phase) reproduces this reliably.
cube_side_length, det_range, odom_only, and timestamp_unit are parsed and forwarded as CLI args but never consumed by the compiled Point-LIO binary: cube_len/DET_RANGE/odom_only are write-only globals (the iVox port dropped the fov-segment logic that used them) and preprocess.cpp hardcodes the ms timestamp scale, ignoring time_unit. Drop them from PointLioConfig, main.cpp arg parsing, and the pcap_to_db CLI.
…nto jeff/feat/jnav_pgo
| if WRITE_LCM: | ||
| if buffered_points: # flush remainder | ||
| downsampled_points, downsampled_intensities = collapse( | ||
| buffered_points, buffered_intensities if have_intensities else [], LCM_VOXEL | ||
| ) | ||
| aggregated_points.append(downsampled_points) | ||
| if downsampled_intensities is not None: | ||
| aggregated_intensities.append(downsampled_intensities) | ||
| # final unified voxel pass over the per-chunk results, then statistical outlier removal | ||
| merged_intensity_chunks = aggregated_intensities if have_intensities else [] | ||
| downsampled_points, downsampled_intensities = collapse( | ||
| aggregated_points, merged_intensity_chunks, LCM_VOXEL | ||
| ) | ||
| print( |
There was a problem hiding this comment.
ValueError crash when lidar stream has zero scans in .pc2.lcm path
collapse(aggregated_points, ...) is called unconditionally at line 779, but aggregated_points is only populated inside if len(buffered_points) >= CHUNK: and the partial-flush if buffered_points: guard above it. If LIDAR_STREAM contains zero scan observations — a truncated or interrupted recording — neither branch fires, so aggregated_points remains []. np.concatenate([]) inside collapse then raises ValueError: need at least one array to concatenate, crashing the .pc2.lcm write phase. Adding a guard before the final collapse (e.g. if not aggregated_points: ...) surfaces the empty-stream case clearly instead of producing an opaque NumPy traceback.
2 participants
gsc_pgo, Ports the PGO / loop-closure stack into the new
jnavlayout, plus a tf-tree feature formemory2stores.