gsc_pgo: online and offline PGO - #2587
Open
jeff-hykin wants to merge 163 commits into
Open
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
June 24, 2026 07:23
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.
Contributor
Greptile SummaryThis PR introduces the
Confidence Score: 4/5
Important Files Changed
Reviews (52): Last reviewed commit: "gsc_pgo: relock rust deps after merging ..." | 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
jeff-hykin
enabled auto-merge (squash)
June 24, 2026 08:35
…c mounts so lookups route through the fed trajectory
…r loop committed right before stopping still gets removed
…hdog raises, settle-based drain); drop RateReplay and scan caps
…them unacked, deadlocking the ack wait)
…default on multi-thousand-keyframe runs)
…nal pose graphs with edge types, total runtime)
…r rejection The 200m candidate-distance gate discarded ~700 genuine revisits on huge_loop with drifted fastlio odom (inflated raw frame makes true revisits appear far apart); with it off, tag spread collapses 59.6->1.81m. Gate never fired on any go2 run, so this is a no-op there. Auto-scaled scan-context range (0 = first-scan extent) validated at the sweet spot for both go2 L1 and mid360 (manual 15/25/45 sweep). Also adds pgo-eval --lidar-tf for scans recorded in a different frame than the odom body (fastlio_lidar in mid360_link vs base_link).
On large graphs the background full-graph GNC solve takes minutes, so the harness settle window (60s) expired while the classification that rejects false closures was still in flight, leaving them committed in iSAM2. Track dispatched-vs-applied GNC sequences, republish the graph while a solve is pending so the settle heartbeat stays fresh, raise the settle cap, and give factor removals the same extra relinearization passes as insertions.
A forced relinearization can throw on a graph mid-outlier-rejection (many conflicting closures being removed); the estimate is still usable and later updates recover, so log and stop refining instead of killing the pipeline.
Removing hundreds of committed-then-rejected false closures leaves iSAM2 with unrecoverable linearization damage. At idle, adopt the batch GNC poses and rebuild iSAM2 from the odometry backbone plus GNC-kept loops so the live end state matches offline finalize.
Once GTSAM throws mid-update, iSAM2 is left unusable and every later update fails. Recover by rebuilding from the odometry backbone plus still-inlier loops (shared with the idle adoption path); stays fatal with location constraints since the rebuild would drop their factors.
Mid-rejection the graph holds hundreds of conflicting false closures; without the Huber kernel the live path applies, a fresh batch update throws too. If GTSAM still throws, rebuild from the backbone alone — pure odometry always solves and the next GNC adoption restores loops.
The per-keyframe classification poll drains the worker channel and discarded the batch poses, so if the last solve landed before the stream ended the idle adoption never fired. Store the newest result and adopt from it, guarded by an adopted-sequence counter.
The rendered map strided to 400 scans, so each scan laid down an isolated set of ground rings that never merged into a surface — the ground read as a lattice of stripes rather than terrain. Accumulate every scan instead and keep one point per 10cm voxel, which is cheaper than the strided map was. The voxel-agreement metric keeps its own stride so its numbers stay comparable. Also swap the isometric height ramp to the cool half of turbo and clip its color limits to percentiles, since the crop window floor sits below any real ground.
The keyframe thresholds, odometry/ICP factor noise, and every ICP gate were module constants, so tuning a recording whose environment the defaults do not suit (narrow corridors, tag-free rigs, heavy LIO drift) meant editing the pipeline. Collect them into an offline_pgo.Tuning dataclass and generate one CLI flag per field.
…ability The shim constructed GncOptimizer and never touched its inlier threshold, so every solve ran GTSAM's built-in 0.99 chi-squared default. The only lever was loop_gnc_var_scale, which is blunt: inflating a loop factor's variance loosens the outlier test but also weakens the surviving edge's pull. Threading the probability through lets the outlier test be tightened on its own. On hotel.db it moves the kept-loop count monotonically (0.01/0.5/0.9/0.99 keep 0/1/4/5 of 8 closures). Also converts eval.rs's to_pgo to a struct literal, which clippy rejected as field_reassign_with_default once the new field was added.
The post_process rrd lost its camera frustums in an earlier refactor, so the tag landmarks had no photo to check them against. Each tag now carries the medoid glimpse's image on a pinhole at the pose it was taken from. Logging that image as a JPEG EncodedImage (what Image.to_rerun() returns) hangs the rerun 0.32 viewer indefinitely, so the raw pixels go in instead. The rrd also ships a blueprint now: a single 3D view, no per-image 2D panels. Landmarks were being skipped entirely on the d455 rig because its intrinsics live in realsense_color_image_camera_info, not camera_info; resolve_camera_info now tries the image-derived name first.
Tuning loop closure meant juggling fourteen separate gates that all trade the same thing off against each other. loop_conservativeness collapses them into a single 0-4 knob, where 2 reproduces the current defaults, 0 accepts nearly any match, and 4 turns every gate on with a tight GNC. It deliberately overlaps the individual loop_* fields: setting it overwrites all of them, and the default of -1 leaves them alone. The gates it skips are the ones that are not accept/reject tradeoffs -- keyframing, the search radius, the odometry variances, and the Huber kernel.
Building an rrd for a recording without raw_april_tags died on the stream lookup rather than falling back to the clouds and trajectories it can draw.
# Conflicts: # dimos/robot/all_blueprints.py # pyproject.toml
The TF service was retired on main (#3169); tf is now a normal topic, so GscPGO needs its own `tf: Out[TFMessage]` port.
Main renamed the `dimos.memory2` package to `dimos.memory`, which broke every gsc_pgo import. Also defers the open3d/cv2/rerun imports into the functions that use them, matching the pattern already used in dimos/mapping, and drops the gsc_pgo lcm-msgs rev pin so the lockfile resolves to the same e7c9428b every other crate in the repo uses instead of carrying a second copy.
# Conflicts: # dimos/robot/all_blueprints.py
main pinned zenoh to =1.9.0 with default-features off (tcp/udp only), so the tls/ws/quic/unixsock link crates and their subtrees drop out of the lock.
Comment on lines
+130
to
+146
| def _publish_corrected(self, odom: Transform | None, ts: float) -> None: | ||
| """Ack each scan with the drift-corrected current pose (world_correction ∘ odom).""" | ||
| if odom is None: | ||
| corrected = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) | ||
| else: | ||
| optimized = self.pgo._world_correction.compose(_transform_to_pose3(odom)) | ||
| t = np.asarray(optimized.translation()) | ||
| q = Quaternion.from_rotation_matrix(optimized.rotation().matrix()) | ||
| corrected = Pose(float(t[0]), float(t[1]), float(t[2]), q.x, q.y, q.z, q.w) | ||
| self.corrected_odometry.publish( | ||
| Odometry( | ||
| ts=ts, | ||
| frame_id=self.config.world_frame, | ||
| child_frame_id=self.config.body_frame, | ||
| pose=corrected, | ||
| ) | ||
| ) |
Contributor
There was a problem hiding this comment.
_transform_to_pose3 in pass-through path
handle_cloud exits early via self._publish_corrected(odom, msg.ts) for all three placeholder conditions: odom is None, odom.translation.is_zero(), and odom.rotation.is_zero(). Inside _publish_corrected, only the odom is None branch is guarded; the other two fall into else and call _transform_to_pose3(odom). When odom.rotation.is_zero() (all-zero quaternion — the "uninitialized" placeholder), tf.to_matrix() produces a NaN or all-zero rotation matrix, and gtsam.Pose3(that_matrix) produces undefined / NaN state. The published corrected_odometry is then garbage, and on a Go2 recording (which regularly publishes placeholder poses at startup) this fires before any keyframes exist.
Comment on lines
+130
to
+134
| def _publish_corrected(self, odom: Transform | None, ts: float) -> None: | ||
| """Ack each scan with the drift-corrected current pose (world_correction ∘ odom).""" | ||
| if odom is None: | ||
| corrected = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) | ||
| else: |
Contributor
There was a problem hiding this comment.
_publish_corrected should mirror the one in handle_cloud. Currently only odom is None is handled safely; the odom.translation.is_zero() and odom.rotation.is_zero() cases slip into the else branch and call _transform_to_pose3 with an invalid quaternion.
Suggested change
| def _publish_corrected(self, odom: Transform | None, ts: float) -> None: | |
| """Ack each scan with the drift-corrected current pose (world_correction ∘ odom).""" | |
| if odom is None: | |
| corrected = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) | |
| else: | |
| def _publish_corrected(self, odom: Transform | None, ts: float) -> None: | |
| """Ack each scan with the drift-corrected current pose (world_correction ∘ odom).""" | |
| if odom is None or odom.translation.is_zero() or odom.rotation.is_zero(): | |
| corrected = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) | |
| else: |
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
3 participants
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.
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
Examples