Stereo Visual Odometry

The Isaac SDK includes Elbrus Visual Odometry, a codelet and library determining the 6 degrees of freedom: 3 for orientation and 3 for location, by constantly analyzing stereo camera information obtained from a video stream of images.

Tracking speed is effectively real-time, at least 30 fps for 640x480 video resolution. Accuracy is based on the KITTI benchmark http://www.cvlibs.net:

  • drift of ~1% in localization
  • error of 0.003 degrees/meter of motion

Elbrus allows robust tracking across different types of motion:

  • indoor
  • outdoor
  • aerial
  • HMD
  • automotive
  • robotics
../../../_images/stereo_vo_image2.jpg

Architecture

Elbrus employs industry standard keyframes based the SLAM architecture. The end-to-end tracking pipeline contains two major components: 2D and 3D.

The 2D component performs detection of well-distributed 2D features (tracks) in the frames that it marks as a keyframes, and then tracks those feature locations in between keyframes using Lucas-Kanade (LK), a multi-pyramid algorithm that transfers found tracks as 2D positions with unique IDs for every track to the 3D component of the solver. In stereo tracking, the 3D component feeds back to LK tracker predictions based on current results of camera 3D tracking and a “velocity constancy” camera motion model. This improves and accelerates LK tracking and helps to purge 2D tracking outliers.

2D tracking is the most time-consuming part of the Elbrus tracking pipeline. In the case of stereo images, the 2D component consumes over 90% of the time in the CPU reference implementation. To overcome this obstacle to real-time tracking, the most time consuming 2D components (building image pyramid, smoothing spatial derivatives and other 2D convolution image processing routines) perform tracking of all frames from the left camera and between left and right camera images in the key frames of the video sequence.

The 3D component performs 3D tracking. It finds the left camera pose (6 DOF) using a combination of triangulation, resectioning (finding camera pose using known 3D locations - landmarks - and their 2D projections in the camera image plane - tracks) and Sparse Bundle Adjustments (SBA) for stereo cases. Camera poses and all 3D algorithmic work in Elbrus is done using Exponential Mapping (Twists) as 6-DOF camera pose representations.

Source Code

The Isaac SDK includes Elbrus tracker code in the form of a dynamic library, wrapped by a codelet. The Isaac codelet wrapping Elbrus stereo tracker takes a pair of input images, and camera intrinsics. The camera pose is represented by a quaternion and a translation vector describing camera orientation and location-in-the-world coordinates.

Embedded Undistortion

As all cameras have lenses, lens distortion always takes place. Most modern, so-called “undistorted” cameras perform undistortion in software. This consumes CPU/GPU computation power, increases latency, and causes blur in the output image.

Isaac SDK includes this method of undistortion too in the form of the ImageUndistortion GEM. However, a more efficient way is to use Elbrus embedded undistortion on raw (distorted) camera images. Elbrus can track 2D features on distorted images and limit undistortion to selected features in float coordinates. This is much faster and more accurate than performing undistortion of all pixels before tracking.

To use efficient Elbrus undistortion, set left.distortion and right.distortion (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: 4 radial (r0, r1, r2, r3) distortion coefficients

See DistortionProto documentation in ref:messages_api_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.

Running the Sample Application

The Elbrus sample application uses a ZED stereo camera. First, connect the ZED camera to the host system or the Jetson platform you are using.

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

Use the following procedures to run the sample application.

To Run the Sample Application on the Host System

  1. Build the sample application with the following command:

    bob@desktop:~/isaac$ bazel build apps/samples/stereo_vo
    
  2. Run the sample application with the following command:

    bob@desktop:~/isaac$ bazel run  apps/samples/stereo_vo
    

To Run the Application on Jetson

  1. Deploy //apps/samples/stereo_vo:stereo_vo-pkg to the robot as explained in Deploying and Running on Jetson.

  2. Log on to the Jetson system and run the application with the following commands:

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

    Where “bob” is your user name on the host system.

To View Output from the 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 platform, make sure to use the IP address of the Jetson system instead of localhost.