cuVSLAM - #3391
Draft
jeff-hykin wants to merge 73 commits into
Draft
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.
Codecov Report❌ Patch coverage is @@ Coverage Diff @@
## main #3391 +/- ##
==========================================
- Coverage 76.08% 75.72% -0.37%
==========================================
Files 1190 1195 +5
Lines 115283 116019 +736
Branches 10366 10712 +346
==========================================
+ Hits 87714 87851 +137
- Misses 24556 25154 +598
- Partials 3013 3014 +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:
|
jeff-hykin
changed the title
Left on, auto-exposure halves the color frame rate in low light to buy a longer exposure, and the frameset then carries the same color frame twice. Off by default, so the rate holds and the image darkens instead.
jeff-hykin
commented
Aug 8, 2026
| self._profile = None | ||
| self._align = None | ||
| self._color_to_depth_extrinsics = None | ||
| if self._dropped_frames or self._repeated_frames: |
Member
Author
There was a problem hiding this comment.
repeated frames are a sucky part of the realsense API realsenseai/librealsense#1686 (comment)
Extrinsics come off the device graph now, so they are there whether or not depth is streaming and colour never had to fall back to the camera_link origin.
jeff-hykin
commented
Aug 8, 2026
| color_image: Out[Image] | ||
| depth_image: Out[Image] | ||
| infrared_left: Out[Image] | ||
| infrared_right: Out[Image] |
Member
Author
There was a problem hiding this comment.
(1) Need left/right camera for stereo slam
The pipeline quietly substitutes rates it can do, and cuVSLAM computes expected samples per frame from the declared one and stops fusing when they never arrive. Read it back off the active profile and say so when it differs.
jeff-hykin
commented
Aug 8, 2026
| return f"{self._name}_infra1_frame" | ||
|
|
||
| @property | ||
| def _infra1_optical_frame(self) -> str: |
Member
Author
There was a problem hiding this comment.
(3) frames for stereo cameras to measure space between lenses
jeff-hykin
commented
Aug 8, 2026
| depth_image: Out[Image] | ||
| infrared_left: Out[Image] | ||
| infrared_right: Out[Image] | ||
| imu: Out[Imu] |
jeff-hykin
commented
Aug 8, 2026
| self._profile: rs.pipeline_profile | None = None | ||
| self._imu_pipeline: rs.pipeline | None = None | ||
| # Bounded so a stalled accelerometer cannot grow the queue. | ||
| self._accel_history: deque[tuple[float, tuple[float, float, float]]] = deque(maxlen=2) |
Member
Author
There was a problem hiding this comment.
accel and gyro don't show up at the same time, interpolate (it matters) to get them synced for an IMU message
jeff-hykin
commented
Aug 8, 2026
| if not self._running: | ||
| break | ||
| consecutive_timeouts += 1 | ||
| if consecutive_timeouts == 1 or consecutive_timeouts % 5 == 0: |
Member
Author
There was a problem hiding this comment.
prevent quick restart from crashing from camera being busy
imu_accel_hz and imu_gyro_hz become imu_hz, the gyro rate and so the Imu output rate; the accelerometer takes the fastest the device offers, since it is interpolated onto the gyro anyway. A rate the camera does not offer now raises with the list of what it does, rather than silently substituting. _frame_extrinsics only ever crossed one function boundary, and the extrinsics are read against whichever stream comes first rather than bailing when depth is absent. Measured that starting a pipeline does not reset global_time_enabled, so the second call was doing nothing.
… them cuVSLAM requires Track() and RegisterImuMeasurement() in non-decreasing timestamp order, and the dispatcher drains one message per input per round, so a 400 Hz IMU was being handed samples older than the frame already tracked. IMU now buffers in the module and flushes up to each frame's stamp. Depth took its frame_id from align_depth_to_color rather than from whether the aligner exists, so with colour disabled it was stamped a frame nothing publishes. Giving up on video left the module publishing camera_info forever with no images. Comments: 62 fewer lines of them.
driver_cuda_major() dlopened libcuda by bare name, which a nix-built python resolves nowhere -- the same loader gap the module already works around for its own subprocess -- so variant selection fell back to cuda12 on exactly the machines that needed the logic. Absolute paths into the known driver dirs now back the bare name up, a stale symlink from a driver upgrade is re-pointed, and a pre-12 driver gets told cuVSLAM ships nothing it can run.
Adds enable_right_image, the right optical frame and its tf edge off the factory baseline, and a demo-zed-cuvslam blueprint that fans both eyes onto the cuvslam image stream. enable_depth=False now sets DEPTH_MODE.NONE so grab() stops running depth inference nothing reads.
NVIDIA ships no macOS cuVSLAM build, so this points the flake at our own v17.0.0 build for Apple silicon, compiled against CuMetal and published as a release on jeff-hykin/cuda-metal. A fresh Mac with only nix installed can now `nix build .` here and track on the GPU: the archive carries every kernel already lowered to a metallib, wired up through CUMETAL_PREBUILT_CACHE_DIR, so no Xcode or Metal compiler is involved.
…to jeff/feat/cuvslam
…c handler The assembly passed camera_name="d455", which overrides the model the camera already detects, so a D435 on the same rig published d455_* frames. Root the static mount tree at base_link instead and let the camera place its own link, which is what base_transform is for. OdometryPath drops its start() override and manual subscribe for async def handle_odometry.
It is a count of poses in the pose graph, not a size in metres.
Same effective default: slam_async=False is the old sync_mode=True. demo_cuvslam -> demo_cuvslam_realsense, demo_zed_cuvslam -> demo_cuvslam_zed.
Replaces the lopsided color_image + right_image pair. With stereo off the module publishes color_image as before; with it on both eyes go out on left/right ports and the frames are named to match.
jeff-hykin
commented
Aug 8, 2026
| 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
Author
There was a problem hiding this comment.
iterpolating acceleration to match gyro time
It opened a second rs.context() and re-enumerated USB while the pipeline was streaming, and took the first device when serial was None. The infra profiles fetched a few lines above carry the same extrinsic.
jeff-hykin
commented
Aug 9, 2026
| quat = Rotation.from_matrix(rotation_matrix).as_quat() # [x, y, z, w] | ||
| return Transform( | ||
| translation=Vector3(*extrinsics.translation), | ||
| rotation=Quaternion(quat[0], quat[1], quat[2], quat[3]), |
Member
Author
There was a problem hiding this comment.
This doesn't take into account the reference frame that the realsense is publishing the transform from. E.g. "turn right". Its only ~15mm off for the 435 but it becomes a problem for stereo SLAM. Gotta convert to body, apply, then apply body inverse.
jeff-hykin
commented
Aug 9, 2026
|
|
||
| # camera_link -> camera_depth_frame (identity, depth is at camera_link origin) | ||
| camera_link_to_depth = Transform( | ||
| translation=Vector3(0.0, 0.0, 0.0), |
Member
Author
There was a problem hiding this comment.
All of these should be derived from the sensor data itself (and it is now in _mount_edges)
jeff-hykin
commented
Aug 9, 2026
| ${CUVSLAM_LIBRARY} | ||
| ) | ||
| target_link_directories(cuvslam_odometry PRIVATE ${LCM_LIBRARY_DIRS}) | ||
| if(APPLE) |
Member
Author
There was a problem hiding this comment.
Yep, working on MacOS 😁 CuMetal project
jeff-hykin
commented
Aug 9, 2026
| # name resolves everywhere else. | ||
| candidates = ["libcuda.so.1"] + [ | ||
| str(directory / "libcuda.so.1") | ||
| for directory in (*_DRIVER_ONLY_LIB_DIRS, *_HOST_LIB_DIRS) |
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.
Stereo visual odometry via cuVSLAM.
Adds
CuvslamOdometry(aNativeModule, so the cuVSLAM SDK is fetched and built by nix on firstbuild()) andOdometryPath. The RealSense and ZED camera modules gain stereo output and read their frame geometry off the device.Run
Two blueprints, one per camera. Both want the camera plugged in over USB3.
RealSense (D455 / D435i):
ZED:
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.