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.
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.
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.
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.
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
(see ColorCameraProto) 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.
The “stereo_vo” sample application uses the ZED camera, which performs software undistortion inside the StereoLabs SDK.
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
option in the StereoVisualOdometry GEM for denoising with increased tracking speed and accuracy.
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.
Isaac SDK includes the following sample applications, which demonstrate Stereo VIO integration with third-party stereo cameras that are popular in the robotics community:
apps/samples/stereo_vo/svo_zed.py: This Python application demonstrates Stereo VIO integration with the ZED and ZED Mini (ZED-M) cameras.
apps/samples/stereo_vo/stereo_vo.app.json: This JSON sample application demonstrates SVIO integration with the IMU-equipped ZED-M camera.
apps/samples/stereo_vo/svo_realsense.py: This Python application demonstrates SVIO integration with the Intel RealSense 435 camera.
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.
If you want to use a regular ZED camera with the JSON sample application, you need to edit the
apps/samples/stereo_vo/stereo_vo.app.json application before running it:
Change the codelet configuration parameters
stereo_vo/stereo_vo/process_imu_readings from “true” to “false”.
Running the 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$ bazel run apps/samples/stereo_vo:svo_zed
Build and run the Python sample application for the ZED-M camera with the following command:
bob@desktop:~/isaac$ bazel run apps/samples/stereo_vo:svo_zed_imu
Build and run the JSON sample application for the ZED-M camera with the following command:
bob@desktop:~/isaac$ bazel run apps/samples/stereo_vo:stereo_vo
Build and run the Python sample application for Realsense 435 camera with the following command:
bob@desktop:~/isaac$ bazel run apps/samples/stereo_vo:svo_realsense
bob is your username on the host system.
Running the Sample Applications on a Jetson Device
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
//apps/samples/stereo_vo:stereo_vo-pkg) to Jetson, log in to the Jetson system and run the
JSON sample application with the following
bob@jetson:~/$ cd deploy/bob/stereo_vo-pkg bob@jetson:~/deploy/bob/stereo_vo-pkg$ ./apps/samples/stereo_vo/stereo_vo
To build and deploy the Python sample for ZED and ZED-M cameras
//apps/samples/stereo_vo: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 apps/samples/stereo_vo/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 apps/samples/stereo_vo/svo_zed.py --imu
To build and deploy the Python sample for the Realsense 435 camera
//apps/samples/stereo_vo: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 apps/samples/stereo_vo/svo_realsense.py
bob is your username on the Jetson system.
To View Output from the Application in Websight
While the application is running, open Isaac Sight in a browser by
http://localhost:3000. If you are running the application on a Jetson, use
the IP address of the Jetson system instead of
You should see a similar picture in Sight as shown below; note the colored camera frustrum shown in the Camera Pose 3D view.