Inaccurate odometry (camera_info R matrices composed into rig extrinsics with rectified_images=true; uniform ~1.33x translation scale overestimate)

Robotics & Edge Computing Robotics - Isaac Isaac ROS
1

Setup

  • Jetson AGX Orin, JetPack (Isaac ROS dev container, aarch64)
  • Isaac ROS Visual SLAM 3.2, cuVSLAM 12.6
  • ROS 2 Humble
  • Camera: Luxonis OAK-D Pro W (wide FOV), depthai_ros_driver, on-device rectified stereo pair, 1280x720 @ 30 FPS
  • Vehicle: a wheel loader, camera pitched 14 deg down, stereo baseline 75 mm
  • num_cameras: 2, camera_optical_frames set, rectified_images: true, IMU fusion disabled

Issue 1 : camera_info R is composed into the rig even with rectified_images: true

The depthai ros driver publishes rect-mode camera_info with a malformed R (raw rectification rotations with the diagonal overwritten to 1.0). Using the debug dump (enable_debug_mode: true), I found those R matrices composed verbatim into the stereo.edex per-camera transforms, creating a phantom ~1.55 deg relative rotation (vergence) between cameras whose images are actually rectified parallel:

cam0 transform row 0: [0.99990, -0.005295,  0.012847, -0.0375]
cam1 transform row 0: [0.99990,  0.001139, -0.014210,  0.0375]

At fx=565 this is a ~15 px disparity bias. Depending on sign it produced either depth-dependent scale compression (odometry reporting 10-30% of true displacement) or rejection of essentially all stereo features (observations_cloud width 0-1, constant tracking loss).

Sanitizing camera_info before the node (R := identity, D := 0, unified intrinsics, Tx rescaled to preserve baseline) fixes the rig:

cam0/cam1 transform row 0: [1, ~0, ~0, +/-0.0375]   (plus shared 14 deg pitch in rows 1-2)
horizontal_stereo_camera: 1   (was 0 before the fix)
distortion_params: all zero

After this, tracking is continuous and healthy. Question for the Isaac ROS team: is composing camera_info R into rig extrinsics with rectified_images: true intended? If yes, the requirement that R be identity for pre-rectified input would be worth documenting prominently, since at least one major driver publishes non-identity R in rect mode.

Issue 2 : residual uniform ~1.33x translation overestimate

With the corrected rig (verified in stereo.edex: baseline 0.0751 m, identity relative rotation, zero distortion, unified fx=565.32):

  • A measured 60 m straight drive reports ~80 m displacement in /visual_slam/tracking/odometry — ratio ~1.33, reproduced across two drives.
  • Runs so far had enable_localization_n_mapping: true and enable_ground_constraint_in_odometry/slam: true on a flat farm terrain; z is pinned to exactly 0 with VO-only.
  • map → odom jumps erratically during these runs (observations_cloud width ranges from 24 to 160).

Questions:

  1. With enable_localization_n_mapping: true, does /visual_slam/tracking/odometry include SLAM/relocalization corrections, or is it pure VO in the odom frame? (i.e., can bad relocalizations inflate apparent displacement on this topic?)

  2. Can enable_ground_constraint_in_odometry: true on genuinely 3D terrain distort estimated translation scale / path shape, given it forces z=0 and roll=pitch=0 while the vehicle actually pitches and climbs?

  3. Is there any known path by which a uniform ~1.33x scale error survives a rig with verified baseline and identity relative rotation — e.g., cuVSLAM using an effective focal length different from camera_info K/P for pre-rectified wide-FOV input (alpha/FOV scaling), or resolution/intrinsics mismatch handling?

  4. Recommended settings for repetitive outdoor agricultural environments to suppress spurious relocalization (and whether masking sky regions via img_mask_top is the suggested way to keep features off moving clouds)?

    @Raffaello Could you please help in clarifying the above mentioned issues?

2

Hello @jagruth.medavarapu,

Welcome to the Isaac ROS forum and thanks for the detailed analysis.

For the Issue 1, your interpretation matches the current Isaac ROS Visual SLAM behavior.
When rectified_images=true, the Visual SLAM ROS wrapper does read CameraInfo.R and composes it into the camera extrinsics before passing the rig to cuVSLAM. So if the incoming rectified image stream publishes a malformed or unexpected non-identity R, that rotation can be applied to the rig and can create the phantom stereo vergence you observed.
For already-rectified stereo input, the CameraInfo and TF must describe the exact images being sent to Visual SLAM. In this case, your workaround make sense.
I’ll share your feedback with our internal team and check how we can clarify this in the documentation. Thanks for pointing this out!

3

For Issue 2, I would first isolate the remaining scale error from SLAM corrections and ground constraints.

From the Visual SLAM wrapper, /visual_slam/tracking/odometry is published from the VO pose in the odom frame. When enable_localization_n_mapping=true, the SLAM pose is used to compute/publish map -> odom, so relocalization or loop-closure behavior can cause jumps in map -> odom.
Those jumps should not directly change the odometry message itself, but they can affect any displacement measured through the full TF chain in the map frame.
Also, enable_ground_constraint_in_odometry=true modifies the VO pose to constrain motion to a horizontal plane.

For debugging a scale issue, I recommend disabling it first, especially on outdoor terrain where the vehicle can pitch or climb. Could you try one diagnostic run with:

  • enable_localization_n_mapping=false
  • enable_ground_constraint_in_odometry=false
  • enable_ground_constraint_in_slam=false
  • sanitized rectified CameraInfo

Then measure the 60 m drive directly from /visual_slam/tracking/odometry or /visual_slam/tracking/vo_pose, not from the map -> odom -> base_link TF chain.

One calibration point to double check: Visual SLAM reads intrinsics from CameraInfo.K, not from CameraInfo.P. In ROS CameraInfo, P describes the processed/rectified image and may differ from raw K. For pre-rectified images, please make sure K contains the rectified intrinsics that match the actual image stream. Do not rely only on P being correct.