cuVSLAM - #3391
Open
jeff-hykin wants to merge 100 commits into
Open
Conversation
…module Packages NVIDIA's official cuVSLAM 17.0.0 C++ SDK with nix (fetchurl + pinned hash + autoPatchelfHook) and wraps it as a dimos native module, plus a replay demo that drives it from a memory2 recording and builds a map. The module tags every pose with a segment id in child_frame_id, because cuVSLAM restarts its world frame after a tracking loss; differencing poses across that boundary reads a frame change as motion. libcudart dlopens libcuda.so.1, which the nix loader cannot find on a non-NixOS host, and reports it as "driver version is insufficient". CuvslamConfig puts a driver-only lib directory on LD_LIBRARY_PATH to fix that.
…teleport Two bugs, both found by looking at the map. GetLastLandmarks() returns points in the last camera frame, not the world frame, so publishing them unchanged piled every frame's points around the origin instead of drawing the room. The struct doc claims world frame and the method doc claims camera frame; transforming by world_from_rig is what produces a map with walls in it. cuVSLAM also restarts its world frame without ever returning an empty pose, so keying the segment id on a missing pose never fired and the published stream contained a 94 m teleport. Detect the restart from a step no robot could have travelled, rebase onto the last published pose, and bump the segment id. Odometry is now continuous at a variable rate, which is what a live consumer needs; the segment id remains as the diagnostic for the unmeasured gap. Max step over the airbnb replay drops 94.288 m -> 0.544 m, weighted ATE 0.865 -> 0.440 m, path ratio 1.554 -> 1.212.
child_frame_id was carrying a changing segment id, which is wrong twice over: a robot only ever sees a single odom path, and a varying child_frame_id breaks any tf consumer. It is now the constant "cuvslam_rig" and the reset is logged instead, with its timestamp so a debugging tool can mark it on the path. The demo evaluated per segment, aligning each piece to ground truth independently. That re-anchored the trajectory at every reset and drew the path as disconnected branches with dead ends. It now does one rigid fit over the whole run, which is the honest measure of a continuous odometry stream, and the same single transform carries the landmarks. The reported error rises sharply because the per-segment numbers were flattered by re-anchoring, not because anything regressed: ATE 0.44 m per-segment becomes 2.58 m over the whole path, path ratio 1.21 becomes 1.40.
Follows the PGO native module's pattern: the C++ half stamps the odometry with the frame pair and the python half republishes it as a TFMessage, so tf construction stays in one place. Frames are config rather than literals. map->odom is identity because visual odometry has no global correction, and publish_map_to_odom turns it off for a graph that already has something publishing that edge -- two publishers of one tf edge fight each other. The pose is the left camera's. Publishing it as base_frame assumes the camera is the body origin, which the docstring now says out loud; a real robot should either point base_frame at the camera's own frame or feed in the mount extrinsic.
The module wrapped only cuvslam::Odometry, so map->odom was identity and the landmark map smeared with the drift. Slam now runs alongside: fed Odometry::State each frame, its pose published as corrected_odometry (map->base_link) and as the map->odom correction, with the identity path switched off so one thing owns that edge. Call sequence matches NVIDIA's own cuvslam/tracker.py. Slam must run in sync mode. GetPose() carries no timestamp, so a pose from a thread running behind cannot be paired with the odometry pose it has to be differenced against; async measured 77 m ATE against 0.25 m for the odometry it was supposed to be correcting. max_correction_m bounds what the graph may ask for. On the airbnb recording the pose graph diverges -- reproduced through NVIDIA's python wrapper, where get_all_slam_poses returns a 115 km path for a 50 m walk -- and the raw correction reached 417 m, which on a robot is a teleport across the map. The cause is underneath: the odometry restarts its world frame ~127 times in 223 s, at implied speeds up to 499 m/s, and a pose graph cannot stitch across that. The demo scores Slam only when it covers most of the run. Without that gate a diverged graph reports a wonderful ATE for the 15 seconds it survived.
The Mid-360 paints bright dots on the IR frames, and frames where cuVSLAM restarts its world frame carry ~46k such pixels against ~1.8k on an average frame. That correlation looked like the cause, so this adds the mask: a 3x3 median top-hat plus a dilation, fed to Track()'s masks argument, which is what NVIDIA documents for telling the tracker where not to put features. It does not help. Three full airbnb runs each way, odometry only: masked ATE 4.589 / 2.792 / 2.053 m with 138 / 121 / 107 restarts, unmasked 3.100 / 4.587 / 4.956 m with 99 / 117 / 105. The ranges overlap and restarts are, if anything, higher with the mask. A controlled probe through the python wheel (no mask / mask / inverted mask) shows the masks do reach the tracker -- inverting them drops 219 of 1498 tracked frames -- so this is a real negative result rather than a no-op. Kept and documented, default off. Also sets CMAKE_BUILD_TYPE=Release. It was unset, so the module had been building with no optimisation at all; the mask took 700 s per run until the build type was fixed and the per-pixel nth_element was replaced with a 19-op median network, after which the same run takes ~195 s.
…rking cuVSLAM inertial mode was running as pure visual odometry. Three faults: - ImuCalibration.frequency must be the rate actually fed. cuVSLAM derives expected samples as frequency * frame_delta and treats the shortfall as lost IMU, so over-declaring it makes inertial alignment silently never initialise. - Odometry::Config::async_sba races: the resulting std::out_of_range is thrown from cuVSLAM's background SBA thread, so no caller-side catch can see it. Disabling it removes the abort and makes tracking deterministic. - rig_from_imu takes Kalibr's T_cam_imu as-is; an earlier 180 deg X flip diverged once fusion actually ran. manifest_value() read "rectified": true with strtod and got 0.0, so rectified_stereo_camera was disabled on every run. Added manifest_flag(). bench_cuvslam gains --imu-freq/--imu-offset/--imu-quat/--sync-sba/--verbosity/ --state-debug/--dump-edex/--start-frame. SetVerbosity is what surfaces cuVSLAM's own warnings; without it the misconfiguration is invisible. orbslam3_runner stages a recording into the EuRoC layout, runs stereo and stereo-inertial, and writes trajectories the comparison page picks up.
…e test cuvslam_native/ had accumulated benchmarking scaffolding that no blueprint can reach. Moved it out rather than deleted, so the numbers on the comparison page stay reproducible: export_replay.py -> dimos/mapping/cuvslam_replay_export.py score_traj.py -> dimos/mapping/cuvslam_score_traj.py bench_cuvslam.cpp -> dimos/mapping/benchmarks_cpp/ demo_cuvslam_replay.py is gone. It drove the tracker through the module and froze partway through every recording, so what it timed was the transport rather than cuVSLAM, and it carried its own copy of umeyama, the evaluator and a matplotlib renderer that topdown_html already provides. It was benchmark.py's only method, which is now empty and points at the harness that replaced it. The module is left at 873 lines: cuvslam.py, cuvslam_odometry.cpp and the nix build. test_cuvslam.py asserts the module's streams and their payload types. That is the failure this module actually had -- it was wired into no blueprint and nothing noticed -- and a renamed stream would silently unwire it again. Running the binary needs the SDK, so that test is self_hosted and skips when the nix output is absent. Also here, from benchmarking the six d455 recordings: - combined_html: heat map, global toggles, yaw-aligned RTAB-Map paths - orbslam3_runner: EuRoC staging and scoring for ORB-SLAM3 - import_rtabmap_html: pull RTAB-Map trajectories out of its Plotly export
Ports the D455's infrared stereo pair, its IMU and their camera_info from the
recorder work, which is what cuVSLAM tracks on. dimos4's RealSense module only
published colour, depth and pointcloud, so there was no stereo pair to feed a
visual odometry module at all. Also brings across the capture-loop fix:
wait_for_frames() raises RuntimeError on timeout as well as on a stopped
pipeline, and treating both as stopped silently ended capture for a whole run.
alfred_cuvslam runs the same robot as alfred_nav with no lidar. Two settings
differ from the module defaults because measurement put them there:
enable_imu=False feeding the D455 IMU made cuVSLAM worse on every one of six
recordings, by 4x at jogging pace, and no gravity,
excitation or time-offset correction recovered it.
async_sba=False cuVSLAM's async bundle adjustment thread races; the
std::out_of_range comes from that thread so no caller-side
handler sees it. NVIDIA's own launcher defaults it off.
Loop closure stays on: it is the difference between drifting odometry and a pose
that survives a revisit, at ~25x faster than real time.
The replay iterators yield untyped messages, so every loop variable coming out of them needed annotating; the reported-drift table needed a concrete value type before .get() would type-check; and two comprehensions had to become explicit loops, because a comprehension has its own scope and an annotation outside it does not reach the loop variable.
The format string had eight specifiers for nine arguments, so the size_t landed in the first %.2f and every number after it shifted.
NVIDIA ships a separate archive per architecture and the flake hardcoded the x86_64 one, so the module could not build on an Orin at all. aarch64 gets the orin build, which is Ubuntu 22.04 based to match JetPack 6.
Isolates cuVSLAM from the robot around it, and needs none of the Alfred dependencies -- alfred_cuvslam cannot even be imported without portal installed.
The rig is the left camera, so every pose came out in the optical convention and the whole tf tree read as ninety degrees over. Rotate both ends into REP-103 body axes. Driver detection missed Jetson entirely -- L4T keeps libcuda in an nvidia subdirectory -- which surfaced as the misleading 'driver version is insufficient'. Also put the host CUDA runtime first, since nixpkgs' 12.9 cuSOLVER fails against JetPack's 12.6 driver.
On Jetson libcuda.so.1 is not self-contained -- it NEEDs libnvrm_gpu.so and libnvrm_mem.so, which pull in a dozen more siblings, all living beside it in /usr/lib/aarch64-linux-gnu/nvidia. Symlinking libcuda.so.1 alone into a private directory left those unresolvable under the nix loader, which does not read the host ld.so.cache, so the dlopen failed and cudart reported [WARNING] Your NVIDIA driver supports CUDA 0.0, but cuVSLAM requires at least CUDA 12.6 followed by "driver version is insufficient for CUDA runtime version(35)" on every frame. The 0.0 is the tell: that is not a version mismatch, it is no driver loaded at all. Directories that hold nothing but driver libraries can go on LD_LIBRARY_PATH whole, since they have no libstdc++ to shadow the binary's own. The symlink farm stays for x86, where the driver sits among the system libraries and exposing the directory would shadow it -- and where libcuda.so.1 depends on nothing but libc, so the farm is enough.
Odometry renders as a single pose, so the viewer showed the camera moving with no record of the route behind it. OdometryPath keeps the history and republishes it as a nav_msgs/Path, which draws as a line. The trail is the fastest read on whether cuVSLAM is actually tracking: it should retrace the route you walked, and a world-frame restart shows up as a straight jump across it. Frames arriving in the viewer prove only that the camera works. Fed from odometry rather than the SLAM-corrected pose, so the line stays continuous and the map->odom edge carries the loop-closure jump. Poses under 2 cm apart are dropped -- a stationary robot otherwise piles thousands of identical points on one spot, and the whole trail is re-encoded on every publish, which is also why the publish rate is capped at 10 Hz. The demo overrides the rendering to drop Path.to_rerun's half-metre lift. That default exists to clear a costmap; there is none here, and the camera flies at whatever height you carry it, so the lift would put the trail where the camera never was.
src = ./. put cuvslam.py and demo_cuvslam.py inside the derivation's source, and the derivation is keyed on that source, so editing either one forced a full C++ rebuild before the module could start -- minutes of waiting for a change that touched no C++ at all. Narrowed to what cmake actually reads. Whole directories rather than named files, so a new .cpp under src/ is still picked up without anyone remembering to update this list.
The default blueprint is a lone 3D view, so the stereo pair -- the thing that tells you whether the camera is delivering at all -- was only reachable by hunting through the entity tree. Stack the two IR views down one side and give the 3D world the rest.
❌ 1 Tests Failed:
View the full list of 1 ❄️ flaky test(s)
To view more test analytics, go to the Test Analytics Dashboard |
jeff-hykin
changed the title
Linear (m/s) and angular (rad/s) limits on the raw pose's frame-to-frame motion, sharing the covariance gate's hold-and-rebase machinery. Catches confident teleports the covariance gate misses and needs no trust in the tracker's self-reported covariance; VINS-Mono's failureDetection() gates the same way. Defaults 5 m/s and 12 rad/s, 0 disables either limit.
Replays a recording's stereo pair, camera infos and mount tf on the live topics through the store's shared-anchor replay view, so the tracker under test is byte-for-byte the production module. Mount edges come from the recording's own tf stream minus the moving world edges, restamped to wall clock so they never age out of a live tf buffer.
…dule The committed registry referenced demo_replay_tmp's TrajectorySink, which only exists in the local working tree; test_all_blueprints_is_current regenerates on a clean checkout and failed on the difference.
The previous fix staged the whole working-tree file and re-introduced the scratch blueprint line it was meant to keep out.
transform.hpp carries the quaternion/rigid-transform algebra, msg_convert.hpp the message/SDK conversion boilerplate, depth_reproject.hpp the rgbd depth reprojection; the module file is down to config, callbacks, gates and slam. Constants are ALL_CAPS. warmup_frames never earned its keep -- measured, the effect on results is a lottery, and the exposure ramp it supposedly skips is over before frame ten -- so the config field and skip loop are gone.
cuvslam_replay compiles the real module around a capture transport: a replay log of the exact LCM payloads the live topics would carry is pushed through the module's own decode dispatch, handlers drained record by record, so behaviour matches the wire message for message with no pacing in between. Verified against an LCM replay of the same recording: identical early gate behaviour, path length within 0.3%, and none of the frame drops the paced pipeline suffers. A full recording replays in about a tenth of wall time.
This reverts commit 53af479.
jeff-hykin
marked this pull request as ready for review
August 11, 2026 17:31
jeff-hykin
requested review from
Dreamsorcerer,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
August 11, 2026 17:31
github-actions
Bot
added
the
ready-to-merge
Plumbs through to both Odometry::Config and Slam::Config. Off runs the tracker on the CPU — deterministic, no CUDA — but needs a libcuvslam built with ENFORCE_GPU=OFF (the jeff-hykin/cuVSLAM fork); NVIDIA's stock SDK binaries are GPU-only. Native binary rebuilt (strict config parser requires struct and python config in lockstep).
github-actions
Bot
added
ready-to-merge
TomCC7
reviewed
Aug 13, 2026
| return | ||
| data = motion.get_motion_data() | ||
| # Hardware capture time, not host time. | ||
| ts = motion.get_timestamp() / MILLISECONDS_PER_SECOND |
Member
There was a problem hiding this comment.
I feel like it's better to add a helper function like ms_to_s rather than barebone division
| return # no accelerometer sample past it yet | ||
| self._pending_gyro.popleft() | ||
| span = end_ts - start_ts | ||
| ratio = 0.0 if span <= 0.0 else max(0.0, min(1.0, (ts - start_ts) / span)) |
Member
There was a problem hiding this comment.
interesting that their driver does not handle alignment...
| return # no accelerometer sample past it yet | ||
| self._pending_gyro.popleft() | ||
| span = end_ts - start_ts | ||
| ratio = 0.0 if span <= 0.0 else max(0.0, min(1.0, (ts - start_ts) / span)) |
Member
There was a problem hiding this comment.
if gyro is faster why not match gyro to accel?
| #include <array> | ||
| #include <cmath> | ||
|
|
||
| namespace transform_math { |
Member
There was a problem hiding this comment.
are we considering Eigen? Is that hard to package?
Member
Author
There was a problem hiding this comment.
That's a good idea. Cuvslam already includes it anyways
Member
There was a problem hiding this comment.
might be the wrong place to put this too many realsense specific logic
Member
Author
There was a problem hiding this comment.
Oops, this whole file wasn't supposed to be committed at all.
| /// rgbd only: raw depth units per metre. 1000 for sixteen-bit millimetres. | ||
| double depth_units_per_meter; | ||
|
|
||
| void validate() const { |
Member
There was a problem hiding this comment.
seems like we can validate this from python side?
Member
Author
There was a problem hiding this comment.
Yeah that's probably better.
Member
There was a problem hiding this comment.
Actually maybe worth checking how cli override work... I'm not even sure why we have this validate API. Seems we can always verify config from python side which will be just easier?
|
|
||
| /// parent_frame -> child_frame through their nearest common ancestor. Not | ||
| /// time-aware; the mount tree is rigid. | ||
| std::optional<Transform> tf_lookup(const std::string& parent_frame, |
Member
There was a problem hiding this comment.
pretty weird we don't have this as tf lib function? maybe we just add it?
Member
Author
There was a problem hiding this comment.
Its been on Andrew's backlog for a bit. I feel like if I add a proper C++ tf it probably won't get merged. Maybe I should just do it.
Normally I just convert everything to rust to use tf, but my cuvslam-rs isnt done yet :shruggie:
| } | ||
|
|
||
| /// Place every camera against the rig frame, or nothing until they all resolve. | ||
| void resolve_rig() { |
Member
There was a problem hiding this comment.
this whole function seems to be frame lookup? a bit dirty to me our tf lib should be able to handle frame name based lookup and time synchronization (not the pr's problem I'm just complaining lol
| if (index < 0) { | ||
| ++unplaced_images_; | ||
| DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), | ||
| "cuvslam dropping image with a frame_id not on the rig", |
Member
There was a problem hiding this comment.
I'm actually quite confused here. All images are just from the same camera with only static transform difference right? We should not really have the case where only some of the camera frame is available
Member
Author
There was a problem hiding this comment.
The "rig" is some special term I think. For example wrist mounted camera's are considered "off rig" because all camera's for cuvslam need have static transforms between eachother. Or that is my current understanding from the docs. (E.g. filter out all the wrist cameras)
Member
Author
There was a problem hiding this comment.
We could avoid connecting the camera streams to the cuvslam stream (through .remappings) but thats actually not as clean as one might think: the realsense has 4 camera streams and I often switch between them via config. If we try to use remappings for that then I need a different blueprint for every setup.
Review: replay logic this realsense-shaped does not belong in memory2. The offline replay need is covered by the cvrlog harness; a general recording-replay module can come back as its own design.
github-actions
Bot
removed
the
ready-to-merge
…validation to python - native/cpp gains dimos/native/tf.hpp: StaticTfTree, a rigid frame-name lookup over TransformStamped edges (nearest-common-ancestor walk, depth cap), with its own doctest suite. Eigen joins the SDK's interface deps (find_package with a pinned FetchContent fallback, same as nlohmann). - cuvslam_main drops the hand-rolled quaternion algebra: transform.hpp is deleted, poses are Eigen::Isometry3d end to end, and the frame lookup is the shared StaticTfTree. Every member transform is explicitly identity-initialized (Isometry3d default-constructs uninitialized). - camera_mode validation moves to python, where CuvslamConfig already enforces it as a Literal; the C++ struct check is gone. 62/62 native tests pass; module builds and links clean.
TfTree.get() resolves parent_from_child (as before); TfTree.publish() now stores the edge and forwards it through a caller-provided sink, the same store-and-send shape as python's self.tf.publish. cuvslam_main wires its tf output port as the sink and drops its hand-rolled publish_tf. Also actually adds test_tf.cpp: the previous commit registered it in cmake but the file itself was swallowed by a tests/ pattern in the local git exclude file, which would have broken the native test build. 63/63 pass.
Per review follow-up: instead of the rigid-tree helper, src/utils/tf.hpp now mirrors native/rust/dimos-module/src/tf.rs — per-edge time-sorted buffers with a fixed window and clock-reset handling, nearest-in-time lookups via lookup().at().tolerance().get(), a blocking within(), BFS shortest-path composition over arbitrary frame graphs (composed stamp is the stalest edge), zero-rotation wire guards, per-pair warn throttling, and publish() feeding the local graph plus a sink. Generic (Eigen + std only, injectable sinks) but lives with cuvslam rather than the native SDK, whose tf helper and Eigen dependency this reverts. Its test suite mirrors the rust one's: 29 cases, all passing (CUVSLAM_UTIL_TESTS=ON, or compile test_tf.cpp against doctest+Eigen). Module builds clean via nix.
Per review: the tf client must not depend on Eigen even though the cuvslam wrapper does. Rotation algebra moves into a self-contained Rigid (xyzw quaternion + translation, the algebra transform.hpp used to carry); msg_convert gains the Rigid<->Isometry3d boundary converters, so Eigen stays wrapper-side. The mirrored test suite (29 cases) now compiles against doctest alone.
github.com/jeff-hykin/dim-slam carries the cuVSLAM lineage (all commits, standalone rather than a github fork). x86_64-cuda12 and orin compile it from source at the pinned rev with ENFORCE_GPU=OFF, so one library serves GPU and CPU and use_gpu is a pure runtime switch; cuda13/thor/metal stay on NVIDIA's tarballs. FetchContent deps are pre-fetched from the repo's own hash pins and handed to cmake as writable source-dir overrides; cuNLS's plain-set() stomps of the CUDA compiler and arch list are patched out. Verified earlier on this box and on orin-095: GPU 499/499 frames tracked on both arches, CPU 499/499 fully driverless with bitwise-identical runs.
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
2 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.
Test
Plug in a realsense (455 or 435if)
Walk the camera around and watch
world/pathin rerun — it should retrace the route you took. A world-frame restart shows up as a straight jump across the trail.Expected performance (stereo with loop closure on no gating/rebasing)
ref DIM-1347