Isaac ROS Visual Odometry

This repository provides a ROS2 package that estimates stereo visual inertial odometry using the Isaac Elbrus GPU-accelerated library. It takes in a time synced pair of stereo images (grayscale) along with respective camera intrinsics to publish the current pose of the camera relative to its start pose.

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

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 per meter of motion.

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

This has been tested on ROS2 (Foxy) and should build and run on x86_64 and aarch64 (Jetson).

System Requirements

This Isaac ROS package is designed and tested to be compatible with ROS2 Foxy on Jetson hardware.

Jetson

AGX Xavier or Xavier NX
JetPack 4.6

x86_64

CUDA 10.2+ supported discrete GPU
Ubuntu 18.04+

Note: For best performance on Jetson, ensure that power settings are configured appropriately (Power Management for Jetson).

Docker

Precompiled ROS2 Foxy packages are not available for JetPack 4.6 (based on Ubuntu 18.04 Bionic). You can either manually compile ROS2 Foxy and required dependent packages from source or use the Isaac ROS development Docker image from Isaac ROS Common.

You must first install the Nvidia Container Toolkit to make use of the Docker container development/runtime environment.

Configure nvidia-container-runtime as the default runtime for Docker by editing /etc/docker/daemon.json to include the following:

    "runtimes": {
        "nvidia": {
            "path": "nvidia-container-runtime",
            "runtimeArgs": []
        }
    },
    "default-runtime": "nvidia"

and then restarting Docker: sudo systemctl daemon-reload && sudo systemctl restart docker

Run the following script in isaac_ros_common to build the image and launch the container:

$ scripts/run_dev.sh <optional path>

You can either provide an optional path to mirror in your host ROS workspace with Isaac ROS packages, which will be made available in the container as /workspaces/isaac_ros-dev, or you can setup a new workspace in the container.

Package Dependencies

isaac_ros_common

Note: isaac_ros_common is used for running tests and/or creating a development container.

Rectified vs Smooth Transform

Elbrus provides the user with two possible modes: visual odometry with SLAM (rectified transform) or pure visual odometry (smooth transform).

Using rectified transform (enable_rectified_pose = true) is more beneficial in cases when the camera path is frequently encircled or entangled. Though rectified transform increases computational resource demands for VO, it usually reduces the final position drift dramatically.

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

Coordinate Frames

This section describes the coordinate frames that are involved in the VisualOdomertyNode. The frames discussed below are oriented as follows:

left_camera_frame: The frame associated with left eye of the stereo camera. Note that this is not the same as optical frame.
right_camera_frame: The frame associated with right eye of the stereo camera. Note that this is not the same as optical frame.
imu_frame: The frame associated with the IMU sensor (if available).
fixed_frame: The fixed frame that aligns with the start pose of the stereo camera. The tracked poses are published in the tf tree with respect to this frame.
current_smooth_frame: The moving frame that tracks the current smooth pose of the stereo camera. The poses in this frame represent a smooth continous motion. Note that the frame can drift over time.
current_rectified_frame: The moving frame that tracks the current rectified pose of the stereo camera. The poses in this frame represent an accurate position and orientation. Note that the frame can have sudden jumps.

Quickstart

Create a ROS2 workspace if one is not already prepared:
mkdir -p your_ws/src
Note: The workspace can have any name; these steps use the name your_ws.
Clone this package repository to your_ws/src/isaac_ros_visual_odometry. Check that you have Git LFS installed before cloning to pull down all large files.
sudo apt-get install git-lfs
cd your_ws/src && git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_odometry
Build and source the workspace:
cd your_ws && colcon build --symlink-install && source install/setup.bash
(Optional) Run tests to verify complete and correct installation:
colcon test

This repository comes with pre-configured launch files for a real camera and a simulator.

For RealSense camera

Note: You will need to calibrate the intrinsics of your camera if you want the node to determine the 3D pose of your camera. See here for more details.

Start isaac_ros_visual_odometry using the launch files:
ros2 launch isaac_ros_visual_odometry isaac_ros_visual_odometry_realsense.launch.py
To connect the RealSense camera default tf tree to the current_smooth_frame(default value is base_link_smooth), run the following command:
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 1 base_link_smooth camera_link
In a separate terminal, spin up RViz to see the tracking of the camera:
rviz2
Add the tf tree in the Displays panel of the RViz.
Set the Fixed frame in the Global Options to the name provided in the fixed_frame variable.
Try moving your camera, and you should see something like this:
If you prefer to observe the Visual Odometry output in a text mode, on a separate terminal echo the contents of the /visual_odometry/tf_stamped topic with the following command:
ros2 topic echo /visual_odometry/tf_stamped

For Isaac SIM

Before launching the isaac_ros_visual_odometry node, set up a Isaac Sim ROS2 bridge as described here.
After that, follow this link to open up the ROS Stereo example.
Note: The default stereo offset for the right camera from Isaac Sim is zero. Change the x value to -175.92 and keep the y value to 0.0.

Press Play to start publishing the data from Isaac SIM.
In a seprate terminal, start isaac_ros_visual_odometry using the launch files:
ros2 launch isaac_ros_visual_odometry isaac_ros_visual_odometry_isaac_sim.launch.py
In a separate terminal, send the signal to rotate the robot in the sim as follows:
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.05}}'
In a separate terminal, spin up the RViz to see the tracking of the robot:
rviz2
Add the tf tree in the Displays RViz panel.
Set the Fixed frame in the Global Options to the name provided in the fixed_frame variable.
You should see something like this:
If you prefer to observe the Visual Odometry output in a text mode, on a separate terminal, echo the contents of the /visual_odometry/tf_stamped topic with the following command:
ros2 topic echo /visual_odometry/tf_stamped

Package Reference