Elbrus Stereo Visual SLAM based Localization

Elbrus is based on two core technologies: Visual Odometry (VO) and Simultaneous Localization and Mapping (SLAM).

Visual Odometry is a method for estimating a camera position relative to its start position. This method has an iterative nature: at each iteration it considers two consequential input frames (stereo pairs). On both frames, it finds a set of keypoints. Matching keypoints in these two sets gives the ability to estimate the transition and relative rotation of the camera between frames.

Simultaneous Localization and Mapping is a method built on top of the VO predictions. It aims to improve the quality of VO estimations by leveraging the knowledge of previously seen parts of a trajectory. It detects if the current scene was seen in the past, i.e. a loop in camera movement, and runs an additional optimization procedure to tune previously obtained poses.

Along with visual data, Elbrus can use Inertial Measurement Unit (IMU) measurements. It automatically switches to IMU when VO is unable to estimate a pose–for example, when there is dark lighting or long solid surfaces in front of a camera. Using an IMU usually leads to a significant performance improvement in cases of poor visual conditions.

Elbrus delivers real-time tracking performance: more than 60 FPS for VGA 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_visual_slam_zed_image21.jpg

Elbrus implements a Stereo VIO and 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.

elbrus_visual_slam_arch1.jpg

On the top:

  1. VIO pushes the camera extrinsics and observations to SLAM.
  2. SLAM adds nodes and edges to the Pose Graph (PG).
  3. SLAM creates landmarks and stores them in the Landmark Spatial Index (LSI).

On the bottom, SLAM periodically executes Loop Closure Solving. If it’s successful:

  1. LAM adds edge to the Pose Graph (PG).
  2. SLAM does Pose Graph Optimization.

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.

Simultaneous Localization and Mapping (SLAM) is built above the results of Stereo VIO. It makes an additional optimization procedure on the graph of poses which leads to increase of accuracy, consuming instead some additional resources. Using SLAM is an option and can be disabled.

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 and SLAM 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.

elbrus_visual_slam_pipe1.jpg

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.

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 ElbrusVisualSLAM 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 “elbrus_visual_slam_zed” 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 denoise_input_images option in the ElbrusVisualSLAM 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.

Elbrus provides the user with two possible modes: visual odometry with SLAM (default) or pure visual odometry. Using SLAM is more beneficial in cases when the camera path is frequently encircled or entangled. Though SLAM increases computational resource demands for VO, it usually reduces the final position drift dramatically.

On the other hand, using pure VO may be useful on relatively short and straight camera trails. This choice will reduce CPU usage and increase working speed without causing essential pose drift.

odom_vs_slam1.jpg

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

  • packages/visual_slam/apps/elbrus_visual_slam_zed_python.py: This Python application demonstrates SLAM integration with the ZED and ZED Mini (ZED-M) cameras.
  • packages/visual_slam/apps/elbrus_visual_slam_zed.app.json: This JSON sample application demonstrates SLAM integration with the IMU-equipped ZED-M camera.
  • packages/visual_slam/apps/elbrus_visual_slam.app.json: This JSON sample application demonstrates SLAM integration with the IMU-equipped ZED-M camera.
  • packages/visual_slam/apps/elbrus_visual_slam_realsense.py: This Python application demonstrates SLAM 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/elbrus_visual_slam_zed.app.json application before running it: Change the codelet configuration parameters zed/zed_camera/enable_imu and elbrus_visual_slam_zed/elbrus_visual_slam_zed/process_imu_readings from “true” to “false”.

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.

The Elbrus codelet has two fields for writing poses of the left camera into pose tree:

  • odom_pose - corresponds to poses obtained by visual odometry.
  • slam_pose - corresponds to poses obtained by SLAM.

There are also two distinct publishers of poses for interaction with other codelets:

  • left_camera_vio_pose for visual odometry poses.
  • left_camera_slam_pose for SLAM poses.

If SLAM is disabled, visual odometry poses will be used instead of SLAM poses.

Running the Stereo Visual SLAM based localization Sample Applications on a x86_64 Host System

  • Build and run the JSON sample application for the ZED camera with IMU with the following command:
Copy
Copied!
            

bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_zed

  • Build and run the Python sample application for the ZED camera with the following command:
Copy
Copied!
            

bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_zed_python -- --imu

  • Build and run the Python sample application for the ZED camera without IMU with the following command:
Copy
Copied!
            

bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_zed_python

  • Build and run the Python sample application for Realsense 435 camera with the following command:
Copy
Copied!
            

bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_realsense

Where bob is your username on the host system.

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

  • packages/visual_slam/apps/elbrus_visual_slam_sim_lidar.py: This Python application demonstrates Stereo Visual SLAM based localization integration with the virtual Carter robot and uses the lidar navigation stack. Several command-line arguments can be passed while running this application: For example, use --sim <simulator_name> to select the simulator and --record to record the simulator data. For more information on available command-line arguments you can check the following file: packages/visual_slam/apps/elbrus_visual_slam_sim_lidar.py
  • packages/visual_slam/apps/elbrus_visual_slam_sim_joystick.py: This Python application demonstrates the Stereo Visual SLAM based localization integration with the virtual Carter robot and requires manual control for navigation. This application has bunch of command line arguments that can be passed while running this application. For eg. --sim <simulator_name> to select the simulator, --record to record the simulator data. For more information on available command-line arguments you can check the following file: packages/visual_slam/apps/elbrus_visual_slam_sim_joystick.py This app requires much fewer resources than elbrus_visual_slam_sim_lidar.

The next sections describe the steps to run the sample applications in Isaac Sim Unity3D.

  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:

    Copy
    Copied!
                

    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 elbrus_visual_slam_sim_lidar Isaac application:

    Copy
    Copied!
                

    bob@desktop:~$ cd isaac/sdk bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_sim_lidar

  4. Open http://localhost:3000/ to monitor the application through Isaac Sight.

  5. Right-click the elbrus_visual_slam_sim_lidar - 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. Another option is to switch goals.goal_behavior->desired_behavior from pose to random in the right “Application configuration” toolbox.

  10. Enable the required channels to ensure smooth visualization of the Stereo Visual SLAM 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 SLAM based localization sample application. For the additional details, check the Frequently Asked Questions page.

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:

Copy
Copied!
            

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

elbrus_visual_slam_sim_lidar1.jpg

  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:

    Copy
    Copied!
                

    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 elbrus_visual_slam_sim_joystick application:

    Copy
    Copied!
                

    bob@desktop:~$ cd isaac/sdk bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_sim_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.

The next sections describe how to run the sample applications in Isaac Sim Omniverse.

  1. Complete the steps described in the Getting Started section for Isaac Sim Omniverse.

  2. Launch Isaac Sim Omniverse and open the following scene: omniverse:/Isaac/Samples/Isaac_SDK/Scenario/carter_warehouse_with_forklift_stereo.usd

  3. Start the Robot Engine Bridge by clicking the Create Application button and press play. The bridge should create a new viewport with a total of two viewports, one for each camera (left and right).

  4. Enter the following commands in a separate terminal to run the elbrus_visual_slam_sim_lidar Isaac application:

    Copy
    Copied!
                

    bob@desktop:~$ cd isaac/sdk bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_sim_lidar -- --map=apps/assets/maps/virtual_test_warehouse_1.json --sim=isaac_sim_ov

  5. Open http://localhost:3000/ to monitor the application through Isaac Sight.

  6. Right-click the elbrus_visual_slam_sim_lidar - Map View Sight window and choose Settings.

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

  8. Click Add marker.

  9. 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.

  10. 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. Another option is to switch “goals.goal_behavior->desired_behavior” from “pose” to “random” in the Application configuration toolbox on the right.

  11. Enable the required channels to ensure smooth visualization of the Stereo Visual SLAM 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 sample application. For additional details, see the Frequently Asked Questions page.

elbrus_visual_slam_sim_lidar1.jpg

  1. Complete the steps described in the :ref: ‘Getting Started <getting-started>’ section for Isaac Sim Omniverse.

  2. Launch Isaac Sim Omniverse and open the following scene: omniverse:/Isaac/Samples/Isaac_SDK/Scenario/carter_warehouse_with_forklifts_stereo.usd

  3. Start the Robot Engine Bridge by clicking the Create Application button and press play. The bridge should create a new viewport with a total of two viewports, one for each camera (left and right).

  4. Enter the following commands in a separate terminal to run the elbrus_visual_slam_sim_joystick application:

    Copy
    Copied!
                

    bob@desktop:~$ cd isaac/sdk bob@desktop:~/isaac/sdk$ bazel run packages/visual_slam/apps:elbrus_visual_slam_sim_joystick -- --map=apps/assets/maps/virtual_test_warehouse_1.json --sim=isaac_sim_ov

  5. Open http://localhost:3000/ to monitor the application through Isaac Sight.

  6. 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.

elbrus_visual_slam_zed_image11.jpg


© Copyright 2018-2020, NVIDIA Corporation. Last updated on Oct 30, 2023.