alfred-mls-nav on the single DimSlam module - #3634
Open
jeff-hykin wants to merge 57 commits into
Open
Conversation
jeff-hykin
requested review from
Dreamsorcerer,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
August 24, 2026 05:29
jeff-hykin
marked this pull request as draft
August 24, 2026 05:33
Codecov Report❌ Patch coverage is
@@ Coverage Diff @@
## jeff/fix/ray_tracing_tf #3634 +/- ##
========================================================
Coverage 77.78% 77.79%
========================================================
Files 1293 1296 +3
Lines 123834 123917 +83
Branches 10851 10851
========================================================
+ Hits 96329 96404 +75
- Misses 24364 24373 +9
+ Partials 3141 3140 -1
Flags with carried forward coverage won't be shown. Click here to find out more.
... and 1 file with indirect coverage changes 🚀 New features to boost your workflow:
|
Contributor
Greptile SummaryThe D455 camera-mount calibration is updated consistently in Alfred’s navigation blueprint and URDF. The current stereo input path rejects frames whose left/right timestamps exceed the backend’s 1 ms skew limit before tracking, and the previously reported replay implementation is no longer present. Confidence Score: 5/5No blocking failure remains. The updated base-to-camera transform matches between the blueprint and URDF after representation conversion. The checked stereo late-frame sequence is rejected before tracking, and the removed replay path cannot execute.
 T-Rex LogsWhat T-Rex did
Reviews (8): Last reviewed commit: "alfred: recalibrate the D455 mount off t..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
jeff-hykin
commented
Aug 24, 2026
| # cuVSLAM alone fits at 1.3 m. Heading is visual deltas with the gyro in | ||
| # between. Visual z is dropped: the CPU tracker's z drifts metres per | ||
| # minute on this rig, and the planar twist constraint below already pins z. | ||
| source_pose_variances=[ |
Member
Author
There was a problem hiding this comment.
Future me double check theres not a cleaner way do to this
jeff-hykin
commented
Aug 24, 2026
| # The CPU-built tracker reports covariance as identity plus accumulated | ||
| # drift, so its translation std starts above 1.0 and grows past 9 during | ||
| # normal driving: any threshold either rejects everything or nothing. | ||
| # Off; the speed gate stays as the teleport backstop. |
Member
Author
There was a problem hiding this comment.
Should probably fix this nowthst we have a fork of cuVSLAM
jeff-hykin
commented
Aug 24, 2026
| transform | ||
| for transform in message.transforms | ||
| if transform.frame_id not in LIVE_PARENT_FRAMES | ||
| and transform.child_frame_id != "base_link" |
Member
Author
There was a problem hiding this comment.
looks bad to me, should just be publishing everything
jeff-hykin
marked this pull request as ready for review
August 24, 2026 23:58
github-actions
Bot
added
the
ready-to-merge
jeff-hykin
force-pushed
the
jeff/feat/alfred_dim_slam
branch
from
August 25, 2026 08:30
f8c3827 to
2582f20
Compare
github-actions
Bot
removed
the
ready-to-merge
jeff-hykin
force-pushed
the
jeff/feat/alfred_dim_slam
branch
from
August 25, 2026 08:50
2582f20 to
34cea65
Compare
The camera pointcloud was a 5 fps stand-in on the lidar stream and needed colour streaming to assemble at all. DimSlam's depth_cloud follows the depth frame rate and carries the range gate, so RayTracingVoxelMap consumes it directly and the camera stops assembling a pointcloud nothing reads.
…intcloud" This reverts commit c55bb83.
The camera's python pointcloud assembly tops out near 5 Hz regardless of pointcloud_fps, and the full-resolution depth_cloud drowned the mapper. DimSlam now publishes the cloud range-gated at 6 m and 3x3-median decimated (42 mm pitch at the gate, under the 50 mm voxel), and RayTracingVoxelMap consumes it in place of the camera pointcloud.
The multiprocessing resource tracker exits only once every copy of its keep-alive fd is closed, and its __del__ waits for that during finalization. rerun_bindings.spawn leaks inheritable fds into a viewer that outlives the run, and the forkserver holds a copy of its own; either one hangs stop() forever. Spawn the viewer through Popen, which closes fds above 2, and terminate the forkserver outright rather than waiting on an EOF that cannot arrive.
alfred-mls-nav carried the whole vision-only stack inline, so anything that wanted to run it against something other than the robot had to copy it and drift. vis_nav holds every sensor-independent module -- DimSlam, the ray tracing voxel map, MLS planning, Dan's planner and controller -- and alfred-mls-nav is now just that plus the RealSense and high-level drivers. DimSlam gains the depth2depth fields the stack passes it, so the decimated cloud is built from the densified depth image rather than the raw one.
AlfredReplay publishes a recording's cameras, IMU and wheel odometry onto the same stream names the live drivers use, so the blueprint is the replay source plus vis_nav verbatim -- a replay exercises the code a real run does. Both imagers are zipped rather than scheduled apart: under decode load they drift past the tracker's 1 ms skew limit and the pair is rejected. Recorded tf edges under odom/map/visual_odom are dropped so the replayed filter owns base_link instead of it having two parents. Each stream is paced against wall time on its own thread, so a consumer slower than the recording stretches the run by an amount nothing can predict; a done-file marks the point where every stream has drained, and a periodic line reports the achieved rate and the spread between the leading and trailing stream, which is what shows up downstream as images older than the tracker's replay buffer.
…lock A slow consumer used to pull its stream ever further behind the anchor, something a live sensor can never do. Frames whose slot has passed are now dropped (decode deferred so drops cost a db fetch, not an image decode), paced by a per-subscription thread with chunked sleeps because macOS timer coalescing multiplies a background process's sleep requests severalfold. Alfred stereo is paired by stamp instead of arrival order, since uncoordinated drops would misalign an ordinal zip forever.
The replay demo and the alfred replay module each carried their own stamp matcher and their own copy of the 1 ms constant. Both now use mapping/dim_slam/stereo_pairing, which gains the tests neither had. The notes corrected: the stereo baseline comes from the recorded tf chain, not camera_info P[3]; the tracker publishes no twist at all rather than a differentiated one; the wheels contribute x/y only; and the D455 mount now points at the urdf instead of naming a different recording than the urdf does.
Nothing imports portal at blueprint-import time, so the entry only stood to swallow a real import failure in alfred-mls-nav. Also trims three comments back to the part the name does not already say.
This reverts commit d36d39a. The cap moves to jeff/fix/mls_planner_span, which stays out of the stack.
The branch now builds the in-repo crate on the plain odom-fusion library instead of dimSLAM's depth2depth branch; densification isn't part of this stack.
jeff-hykin
force-pushed
the
jeff/feat/alfred_dim_slam
branch
from
August 25, 2026 09:03
34cea65 to
549b7c5
Compare
github-actions
Bot
added
the
ready-to-merge
The child offsets were D435 values: a 50 mm stereo baseline where the D455 reports 95 mm, and colour on the wrong side of the left imager. The RealSense module publishes the device extrinsics at runtime, so only the rerun model was wrong. Values read back off the device in the 2026-08-25 spin recording.
Roll, pitch and height come from the spin recording scored against the Mid-360; yaw, x and y stay as measured, since a spin in place leaves those axes flat. Median depth-to-lidar distance improves 8.4 cm to 8.1 cm, inliers 35% to 38%. The blueprint constant carries the same pose as the urdf joint.
github-actions
Bot
added
ready-to-merge
Both IR imagers publish onto one camera_info topic, so cuVSLAM had nothing to order the rig by and fell back to sorting the frame names it had seen. That puts infra1 before infra2, which is left before right only because of how the frames happen to be named.
The D455 motion module reports "No device connected" once the video pipeline holds the device, so the IMU has to be started first. Rate discovery moves off _profile onto a device query, so it also answers before the pipeline is up.
github-actions
Bot
added
ready-to-merge
…l time librealsense builds its device-to-host clock fit by polling the camera over USB. Once the video streams saturate the bus that fit degrades into a rate error, so the motion module's stamps walk away from host time without bound - measured at 0.4-1.6%/s, which put /imu about 24s ahead of the host on Alfred and made every tf lookup against a cloud fail. The raw hardware clock is well behaved (dt 2.495ms +/- 1us, drift 0.007%), so take it and re-anchor it here instead. Delivery latency only pushes an arrival later, so the smallest offset seen recently is the best estimate of the true one. The option has to be set through the IMU pipeline's own device handle; a handle obtained from any other pipeline or context is a different sensor object and setting it there does nothing.
github-actions
Bot
removed
the
ready-to-merge
…orkaround The rate error is librealsense#15360, which is merged but unreleased, and disabling global time is what Intel tells everyone on the related reports. Worth a link so the next reader knows this is a known upstream defect rather than something invented here.
Raw colour was 19.6 MB/s of the Jetson's wire traffic for a stream that is only ever looked at. Encoding it drops that to 1.53 MB/s and raises its rate. SHM is deliberately not used here: shared memory never reaches the wire, so nothing recorded or out-of-process could see the stream, and recording the colour channel is a requirement.
Depth scale, depth-cloud gating and `rectified` describe a camera, not the module, and the noise figures describe an IMU. A second camera or a second IMU had no way to disagree with them. They now live in `cameras`, keyed by the frame_id the images carry, and `imus`, keyed by the frame_id the samples carry. Fusion sources move from three parallel lists to `sources`, keyed by the full "parent->child" transform, so two sources sharing a parent stay apart. `camera_frames` stays a list: it carries cuVSLAM's rig order, and a JSON object's key order does not survive deserialization.
…own lock The rerun bridge was sent every frame of colour, depth and the IR pair, which is far more than a viewer over ssh can take; cap them at roughly 1 Hz each. depth_cloud_decimation moves into the blueprint at the value Alfred was already being launched with. Keying the cameras by frame id put it inside a dict, and the CLI schema only expands nested models, not dict values, so --depth-cloud-decimation no longer parses. The dim_slam flake takes this repo as an input, so writing the lock during evaluation changes the tree being hashed and the build fails on its own edit. ray_tracing already passes --no-write-lock-file for the same reason.
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.
Alfred blueprints for cuVSLAM + wheel odom.
Replay
Real robot