dim_slam: one crate, cuVSLAM feeding fusion in-process - #2
Draft
jeff-hykin wants to merge 14 commits into
Draft
Conversation
cuVSLAM reports covariance_xyz_rpy on the rig frame, but publish() moved only the pose onto base_frame; with a nonzero base_from_rig the 6x6 was in the wrong frame for any downstream consumer weighting by it. Move it through the SE(3) adjoint of base_from_rig, fixed at tracker creation.
Loop closure and relocalization move downstream to the fusion filter; the module is now pure visual odometry. Drops the corrected_odometry output, the map->odom correction, and the slam_* config; map->odom identity stays behind publish_map_to_odom.
15-state error-state Kalman filter (position, velocity, orientation, gyro and accel bias) propagated by the IMU and corrected by nav_msgs/Odometry sources told apart by header.frame_id: the filter's own odom_frame fuses absolutely, any other frame fuses consecutive-pose deltas anchored to the filter. Per source, per dimension variance policy (negative reads the message covariance, zero drops the dimension, positive is fixed), body-frame zero-twist constraint rows for non-holonomic bases, a Mahalanobis gate, and a rollback-replay buffer for late measurements. Filter core is header-only with a doctest suite.
Same design as the C++ module: 15-state ESKF, sources told apart by header.frame_id, per-dimension variance policy, zero-twist constraints, Mahalanobis gate, rollback-replay buffer. The tf output goes through the SDK's Tf client instead of a hand-rolled one, messages come from the generated lcm-msgs crate, and the filter math sits on nalgebra with the test suite ported alongside. Port names shift slightly because the SDK derives them from field names: the multiplexed odometry input is "sources", the output stays "odometry". Builds with cargo against dimos and dimos-lcm git dependencies.
Everything except libcuvslam itself is Rust on the dimos module SDK: tf rig discovery, depth reprojection, IMU buffering, covariance/speed rebase gates. shim/cuvslam_shim.cpp wraps the three C++ calls used (construct, Track, RegisterImuMeasurement) and catches exceptions into error strings, so a Track failure skips the frame instead of aborting.
Crate, binary, CMake target, C++ source and type names all follow.
The flake now builds rust/cuvslam_odometry and rust/odometry_fusion (cargo, vendored git deps) instead of the cmake tree, so src/, CMakeLists.txt, and the dimos-lcm/pfr/dimos-src/lcm-extended inputs go away. Each variant package carries both binaries via symlinkJoin. Also untracks the accidentally committed nix result symlink.
Port of 53cc07d (jeff/feat/imu_info): the five imu_* config scalars are replaced by a sensor_msgs.ImuInfo input, published by the driver the way camera_info is. The IMU frame comes from the message header instead of the first Imu sample. ImuInfo is vendored until dimos-lcm ships it, with a test pinning the packed fingerprint to the lcm hash chain.
The RealSense driver's own cloud carries every far, noisy depth pixel, and depth error grows as range squared, so every consumer ends up rebuilding the same gate. The tracker already holds the depth intrinsics and the depth scale, so it can emit the clean sensor-frame cloud once.
Downstream of the odometry_fusion filter both modules publish odom_frame->base_frame, so they race on the same tf edge; giving cuVSLAM its own root instead leaves base_link with two parents, which is a malformed tree rather than a redundant one. Off, this publishes odometry only and the filter owns the edge alone.
…nd IMU frame A drifting source's anchor was re-set to the corrected filter pose after every accepted update, so the fraction of each delta the gain did not apply was folded into the next anchor and lost. Advancing the anchor along the source's own chain takes cuVSLAM through the filter from 15.99 m to 3.05 m ATE on a 150 s Alfred slice at the module defaults. The IMU was consumed in whatever frame it arrived in. On Alfred that is camera_accel_optical_frame on a rotated mount, so yaw landed on the wrong axis and every IMU combination scored worse than its IMU-free counterpart. It is now rotated into base_frame through the tf tree, falling back with a warning when no transform is available. use_imu makes the IMU optional: with it off the filter seeds level from the first source message and coasts at constant world velocity between measurements, turning at the last reported angular rate. Wheel odometry through the filter with no IMU scores 10.86 m against the raw stream's 10.84 m.
…rustworthy Rotating the IMU into base_frame through the tf tree is the right idea but it made Alfred's wheel+IMU run far worse, 18.61 m to 57988 m ATE, so it cannot stay in. The recording's chain gives base_link -> camera_accel_optical_frame a roll of -66.97 deg, and after applying it the gyro correlates 0.84 with the point-lio yaw rate on x and 0.85 on z, so yaw is split across two axes instead of landing on one. Solving for the yaw axis directly puts it 54.8 deg away from the gravity direction the stationary accelerometer reports, which cannot be true of a rigidly mounted IMU. The mount published in the tf tree and the mount the IMU samples describe disagree, and until that is settled the raw path is the honest default.
… in-process One binary, one Module. VoCore (cuvslam_odometry) and FusionCore (odometry_fusion) become transport-free libraries; the DimSlam module in main.rs wires the tracker's pose stream straight into the filter as a drifting source, so the internal odometry hop never touches the wire. Standalone module binaries are gone; older dimos branches pin older revs of this repo and keep building. Also folds in the no-IMU blend work in eskf.rs and drops the tf-output config (map_frame, publish_map_to_odom, publish_tf) from the vo side -- fusion owns all tf.
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.
dim_slamcrate with one binarysources, split by header.frame_idnix build .#metal/.#orinproduce result/bin/dim_slam