Feat/ivan/recordergo2livox#2266
Closed
leshy wants to merge 34 commits into
Closed
Conversation
Replaces the monolithic pgo_then_voxels with four primitives over mem2 Streams and Transforms: pgo_keyframes(lidar) -> Stream[Keyframe] keyframes_to_corrections(kfs) -> Stream[Transform] (world_corrected <- world_raw) make_interpolator(corrections) -> (ts) -> Transform (SLERP + linear, endpoint-clipped) apply_corrections(stream, corr) -> Stream[T] (shuffles obs.pose) Drift correction is now a first-class, reusable Stream[Transform] that any pose-stamped consumer can apply — same math as before (rigid T_corr = T_global @ T_local^-1 per keyframe, SLERP + linear interpolation between bracketing keyframes), just composable. dimos map --pgo migrates to the new primitives; pgo_then_voxels is deleted. The internal _SimplePGO / PGOConfig / _KeyPose machinery stays in pgo.py and is imported by pgo2.
Renames: pgo.py -> pgo_internals.py (gtsam/ICP machinery) pgo2.py -> pgo.py (public Stream-shaped API) `dimos.mapping.relocalization.pgo` is now the canonical import path (`pgo_keyframes`, `keyframes_to_corrections`, `make_interpolator`, `apply_corrections`, `correction_at`, `Keyframe`). The internal _SimplePGO / PGOConfig / _KeyPose / _icp / _voxel_downsample helpers live in pgo_internals.py and are imported lazily inside pgo.py to keep gtsam off the public-API import path.
Adds test_pgo.py covering pose normalization on Observation, the Transform↔Pose3 conversion helpers, the interpolator edge cases, apply_corrections behavior, and a real-recording smoke test (skipped when data/go2_short.db is absent). Annotates the two map.py helpers so they pass mypy. Removes leftover section divider comments to satisfy the no-section-markers project rule.
The colors-copy block in `PointCloud2.transform` calls `self.pointcloud` to check `has_colors()`, which forces a tensor->legacy conversion on every invocation. With `pgo --full-pgo` rebuilding from hundreds of lidar frames, that hidden allocation dominates the per-frame cost. The colors path was lidar-irrelevant (lidar clouds have no colors anyway) and the feature it was meant to support never landed; remove it so transform() stays a clean numpy round-trip.
…s into feat/ivan/pgo_rewrite
❌ 3 Tests Failed:
View the top 3 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
5 tasks
Contributor
|
This pull request has been automatically marked as stale because it has not had recent activity. It will be closed in 7 days if no further activity occurs. |
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
1 participant
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.
support scripts for #2202
mem2 streams:
odom- go2 odomlidaremits lidar frames in world frame, pose is go2 odomcolor_imagetopic logs as OPTICAL_FRAME derived from go2 odom base_link informationthis ensures all go2 blueprints work
fastlio_odometry- fastlio odomfastlio_lidarlidar frames in world frame, pose for this mem2 entry is set to fastlio odometrythis recording was created by me corecting @KrishnaH96 recording. (topic names, poses for messages)
there is no need to solve how we deal with transforms broadly
(I will in post-rebind image
optical_frameto fastlio odom for fastlio calib,ration no worries about this)for a recorder basically we don't want solution on how to treat frames in general, we want to start running recordings immediately with a few lines like this https://gist.github.com/leshy/56a27ef6eeebc16a5ecd2c2c7229d9ae
I'm sure above needs cleanup (don't have robot to test) but should create topics above
you can convert your recordings into rrd files you can open with rerun like this, they should look like my sample recording
uv run python -m dimos.mapping.loop_closure.utils.map_rrd mid360_sample --out map.rrd --camera-hz 0 && rerun map.rrd- check it out!when recording apriltags pls look at them from a few angles (check my hk_village recordings), make sure to sit in place for a bit for blur to stabilize
the most important thing is that all messages use time.time()
other small tools like summary:
interactive trim tool in the same dir