Stereo Visual Inertial Odometry

Isaac SDK includes the Stereo Visual Intertial Odometry application: a codelet that uses the Elbrus Visual Odometry library to determine the 3D pose of a robot by continuously analyzing the information from a video stream obtained from a stereo camera and IMU readings (if available).

The Elbrus Visual Odometry library delivers real-time tracking performance: at least 30 fps for 640x480 video resolution. For the KITTI benchmark, the algorithm achieves a drift of ~1% in localization and an orientation error of 0.003 degrees/meter of motion.

Elbrus allows for robust tracking in various environments and with different use cases: indoor, outdoor, aerial, HMD, automotive, and robotics.

../../../_images/stereo_vo_image2.jpg

Architecture

Elbrus implements a SLAM architecture based on keyframes, which is a two-tiered system: a minor subset of all input frames are used as key frames and processed by additional algorithms, while the other frames are solved quickly by 2D tracking of already selected observations. The end-to-end tracking pipeline contains two major components: 2D and 3D.

Stereo Visual Inertial Odometry (Stereo VIO) retrieves the 3D pose of the left camera with respect to its start location using imaging data obtained from a stereo camera rig. The stereo camera rig requires two cameras with known internal calibration rigidly attached to each other and rigidly mounted to the robot frame. The transformation between the left and right cameras is known, and time is synchronized on image acquisition.

Elbrus guarantees optimal tracking accuracy when stereo images are recorded at 30 or 60 fps, and IMU angular velocity and linear acceleration measurements are recorded at 200-300 Hz (if available). The inaccuracy of Stereo VIO is less than 1% of translation drift and ~0.03 degree/meter of angular motion error, as measured for the KITTI benchmark, which is recorded at 10 fps with each frame at 1382x512 resolution.

In case of severe degradation of image input (lights being turned off, dramatic motion blur on a bump while driving, and other possible scenarios), additional motion estimation algorithms will ensure acceptable quality for pose tracking:

  • The IMU readings integrator provides acceptable pose tracking quality for about ~< 1 seconds.
  • In case of IMU failure, the constant velocity integrator continues to provide the last linear and angular velocities reported by Stereo VIO before failure. This provides acceptable pose tracking quality for ~0.5 seconds.

Source Code

Isaac SDK includes the Elbrus stereo tracker as a dynamic library wrapped by a codelet. The Isaac codelet that wraps the Elbrus stereo tracker receives a pair of input images, camera intrinsics, and IMU measurements (if available). If visual tracking is successful, the codelet publishes the pose of the left camera relative to the world frame as a Pose3d message with a timestamp equal to the timestamp of the left frame.

If visual tracking is lost, publication of the left camera pose is interrupted until tracking is recovered. After recovery of visual tracking, publication of the left camera pose is resumed, but there’s no guarantee that the estimated camera pose will correspond to the actual pose of the left camera in the world frame. The application using the visual odometry codelet must detect the interruption in camera pose updates and launch an external re-localization algorithm.

Embedded Undistortion

As all cameras have lenses, lens distortion is always present, skewing the objects in the frame. A general-purpose lens undistortion algorithm is implemented in the ImageWarp codelet. However, for visual-odometry tracking, the Elbrus library comes with a built-in undistortion algorithm, which provides a more efficient way to process raw (distorted) camera images. Elbrus can track 2D features on distorted images and limit undistortion to selected features in floating point coordinates. This is considerably faster and more accurate than undistortion of all image pixels performed before tracking.

Note

If you are using other codelets that require undistorted images, you will need to use the ImageWarp codelet instead.

To use Elbrus undistortion, set the left.distortion and right.distortion (see ImageProto) inputs in the StereoVisualOdometry GEM. The following main DistortionModel options are supported:

  • Brown distortion model with three radial and two tangential distortion coefficients: (r0 r1 r2 t0 t1)
  • Fisheye (wide-angle) distortion with four radial distortion coefficients: (r0, r1, r2, r3)

See the DistortionProto documentation for details.

Note

The “stereo_vo” sample application uses the ZED camera, which performs software undistortion inside the StereoLabs SDK.

Embedded Denoising

If your application or environment produces noisy images due to low-light conditions, Elbrus may select too many incorrect feature points. In this case, enable the denoise_input_images option in the StereoVisualOdometry GEM for denoising with increased tracking speed and accuracy.

Inertial Measurement Unit (IMU) integration

Stereo VIO uses measurements obtained from an IMU that is rigidly mounted on a camera rig or the robot base frame. If Visual Odometry fails due to severe degradation of image input, positional tracking will proceed on the IMU input for a duration of up to one second. The IMU integration ensures seamless pose updates as long as video input interruptions last for less than one second.

For IMU integration to work with Stereo VIO, the robot must be on a horizontal level at the start of the application–otherwise the start pose and gravitational-acceleration vector in the world coordinate system (WCS) maintained by the Stereo VIO will be incorrect.

Using the Stereo Camera Sample Applications

Isaac SDK includes the following sample applications, which demonstrate Stereo VIO integration with third-party stereo cameras that are popular in the robotics community:

  • packages/visual_slam/apps/svo_zed.py: This Python application demonstrates Stereo VIO integration with the ZED and ZED Mini (ZED-M) cameras.
  • packages/visual_slam/apps/stereo_vo.app.json: This JSON sample application demonstrates SVIO integration with the IMU-equipped ZED-M camera.
  • packages/visual_slam/apps/svo_realsense.py: This Python application demonstrates SVIO integration with the Intel RealSense 435 camera.

For Visual odometry to operate, the environment should not be featureless (like a plain white wall). Isaac SDK’s SVO analyzes visible features. The alternative is to use sensor fusion methods to handle such environments.

To try one of the ZED sample applications, first connect the ZED camera to your host system or Jetson device and make sure that it works as described in the ZED camera documentation.

To try the RealSense 435 sample application, first connect the RealSense camera to your host system or Jetson device and make sure that it works as described in the RealSense camera documentation.

The steps required to run one of the sample applications are described in the following sections.

Important

If you want to use a regular ZED camera with the JSON sample application, you need to edit the packages/visual_slam/stereo_vo.app.json application before running it: Change the codelet configuration parameters zed/zed_camera/enable_imu and stereo_vo/stereo_vo/process_imu_readings from “true” to “false”.

Running the Stereo Camera Sample Applications on a x86_64 Host System

  • Build and run the Python sample application for the regular ZED camera with the following command:
bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:svo_zed
  • Build and run the Python sample application for the ZED-M camera with the following command:
bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:svo_zed_imu
  • Build and run the JSON sample application for the ZED-M camera with the following command:
bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:stereo_vo
  • Build and run the Python sample application for Realsense 435 camera with the following command:
bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:svo_realsense

Where bob is your username on the host system.

Running the Stereo Camera Sample Applications on a Jetson Device

Note

For details on the host-to-Jetson deployment process, see Deploying and Running on Jetson.

To build and deploy the JSON sample for ZED-M camera (//packages/visual_slam/apps:stereo_vo-pkg) to Jetson, log in to the Jetson system and run the JSON sample application with the following commands:

bob@jetson:~/$ cd deploy/bob/stereo_vo-pkg

bob@jetson:~/deploy/bob/stereo_vo-pkg$ ./packages/visual_slam/apps/stereo_vo

To build and deploy the Python sample for ZED and ZED-M cameras (//packages/visual_slam/apps:svo_zed-pkg) to Jetson, follow these steps:

  • ZED camera: Log on to the Jetson system and run the Python sample application for the regular ZED camera with the following commands:
bob@jetson:~/$ cd deploy/bob/svo_zed-pkg

bob@jetson:~/deploy/bob/svo_zed-pkg$ ./run packages/visual_slam/apps/svo_zed.py
  • ZED-M camera: Log on to the Jetson system and run the Python sample application for the ZED-M camera with the following commands:
bob@jetson:~/$ cd deploy/bob/svo_zed-pkg

bob@jetson:~/deploy/bob/svo_zed-pkg$ ./run packages/visual_slam/apps/svo_zed.py --imu

To build and deploy the Python sample for the Realsense 435 camera (//packages/visual_slam/apps:svo_realsense-pkg), log on to the Jetson system and run the Python sample application with the following commands:

bob@jetson:~/$ cd deploy/bob/svo_realsense-pkg

bob@jetson:~/deploy/bob/svo_realsense-pkg$ ./run packages/visual_slam/apps/svo_realsense.py

Where bob is your username on the Jetson system.

Using the Simulator Sample Applications

Isaac SDK includes the following sample applications demonstrating Stereo Visual Odometry integration with Isaac Sim Unity3D. Note that these applications demonstrate pure Stereo Visual Odometry, without IMU measurement integration.

  • packages/visual_slam/apps/sim_svio.py: This Python application demonstrates Stereo VO integration with the virtual Carter robot and uses the lidar navigation stack.
  • packages/visual_slam/apps/sim_svio_joystick.py: This Python application demonstrates the Stereo VO integration with the virtual Carter robot and requires manual control for navigation.

The next sections describe the steps to run the Stereo Visual Inertial Odometry sample applications in Isaac Sim Unity3D.

Using the sim_svio Simulator Sample Application

  1. Download and extract the Unity Player (“play mode”) build as described in Isaac Sim Unity3D setup instructions.

  2. Launch the Isaac Sim simulation of the medium-warehouse scene with the following command:

    bob@desktop:~isaac_sim_unity3d/builds$ ./sample.x86_64 -screen-quality Medium -screen-width 854 -screen-height 480 --scene medium_warehouse --scenario 2 --framerate 30
    
  3. Enter the following commands in a separate terminal to run the sim_svio Isaac application:

    bob@desktop:~$ cd isaac/sdk
    bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:sim_svio
    
  4. Open http://localhost:3000/ to monitor the application through Isaac Sight.

  5. Right-click the sim_svio - Map View Sight window and choose Settings.

  6. In Settings, click the Select marker dropdown menu and choose “pose_as_goal”.

  7. Click Add marker.

  8. Click Update. The marker will be added to the map. You may need to zoom in on the map to see the new marker. The robot will not immediately begin navigating to the marker.

  9. Click and drag the marker to a new location on the map. The robot will begin to navigate to the marker location. See Interactive Markers for more information.

  10. Enable the following list of channels to ensure smooth visualization of the Stereo Visual Odometry widgets. Avoid enabling all application channels at once as this may lead to Sight lag issues, which happen when an application is streaming too much data to Sight. Visualization of the lidar navigation stack channels is not relevant for the purpose of this Stereo Visual Odometry sample application. For the additional details, check the Frequently Asked Questions page.

    sim_svio/map/occupancy/map
    sim_svio/map/waypoints/waypoints
    sim_svio/map/restricted_area/polygons
    sim_svio/pose_visualization/PoseTrailSvio/pose_trail
    sim_svio/pose_visualization/PoseTrailGt/pose_trail
    sim_svio/pose_visualization/Pose2Comparer/delta.p
    sim_svio/svo.image_viewer/ImageViewer/image
    sim_svio/svo.image_keypoint_viewer/ImageKeypointViewer/keypoints
    sim_svio/svo.landmarks_viewer/LandmarksViewer/landmarks
    sim_svio/svo.tracker/StereoVisualOdometry/current_pose
    sim_svio/svo.tracker/StereoVisualOdometry/pose_trail
    sim_svio/svo.tracker/StereoVisualOdometry/vo.pos_x
    sim_svio/svo.tracker/StereoVisualOdometry/vo.pos_y
    sim_svio/svo.tracker/StereoVisualOdometry/vo.pos_z
    

    Tip

    You can enable all widget channels at once by right clicking the widget window and selecting enable all channels in the context menu.

Important

If you experience errors running the simulation, try updating the deployed Isaac SDK navsim package, which contains the C API and the NavSim app to run inside Unity. Redeploy //packages/navsim/apps:navsim-pkg to Isaac Sim Unity3D with the following commands:

bob@desktop:~$ cd isaac/sdk
bob@desktop:~/isaac/sdk$ ./../engine/engine/build/deploy.sh -p //packages/navsim/apps:navsim-pkg -d x86_64 -h localhost --deploy_path ~/isaac_sim_unity3d/builds/sample_Data/StreamingAssets
../../../_images/sim_svio.jpg

Using the sim_svio_joystick Simulator Sample Application

  1. Download and extract the Unity Player (“play mode”) build as described in Isaac Sim Unity3D setup instructions.

  2. Launch the Isaac Sim simulation of the medium-warehouse scene with the following command:

    bob@desktop:~isaac_sim_unity3d/builds$ ./sample.x86_64 -screen-quality Medium -screen-width 854 -screen-height 480 --scene medium_warehouse --scenario 2 --framerate 30
    
  3. Enter the following commands in a separate terminal to run the sim_svio_joystick application:

    bob@desktop:~$ cd isaac/sdk
    bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:sim_svio_joystick
    
  4. Open http://localhost:3000/ to monitor the application through Isaac Sight.

  5. Use the Virtual Gamepad window to navigate the robot around the map: first, click Virtual Gamepad on the left, then click Connect to Backend on the widget. Select Keypad and use the “wasd” keys to navigate the robot. See Remote Joystick using Sight for more information.

To View Output from an Application in Websight

While the application is running, open Isaac Sight in a browser by navigating to http://localhost:3000. If you are running the application on a Jetson, use the IP address of the Jetson system instead of localhost.

You should see a similar picture in Sight as shown below; note the colored camera frustrum shown in the Camera Pose 3D view.

../../../_images/stereo_vo_image1.jpg