Isaac ROS Visual SLAM

This repository provides a ROS2 package that performs stereo visual simultaneous localization and mapping (VSLAM) and 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 SLAM 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 the 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 featureless 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). For solutions to known issues, please visit the Troubleshooting section below.

Elbrus SLAM

All slam-related operations work in parallel to visual odometry in a separate thread. Input images get copied into GPU and then Elbrus starts tracking.

Elbrus uses the following:

2D features on the input images
Landmarks - The patch of pixels in the source images with coordinates of the feature assosicated with the position of the camera from where that feature is visible.
PoseGraph: A graph that tracks the poses of the camera from where the landmarks are viewed.

Landmarks and PoseGraph are stored in a data structure that doesn't increase its size in the case where the same landmark is visited more than once.

Imagine a robot moving around and returning to the same place. Since odometry always has some accumulated drift, we see a deviation in trajectory from the ground truth. SLAM can be used to solve this problem. During the robot's motion, SLAM stores all landmarks and it continuously verifies if each landmark have been seen before. When Elbrus recognizes several landmarks, it determines their position(from the data structure). The so-called loop closure occurs, so named because at this moment a connection is added to the PoseGraph, which makes a loop from the free trail of the poses. Immediately after that, Elbrus performs a graph optimization that leads to the correction of the current odometric pose and all poses in the graph. The procedure for adding landmarks is designed such that if we do not see a landmark in the place where it was expected, then such landmarks are marked for deletion and will get deleted. This allows you to use Elbrus over a changing terrain.

List of Useful Visualizations:

visual_slam/vis/observations_cloud - Point cloud for 2D Features
visual_slam/vis/landmarks_cloud - All mapped landmarks
visual_slam/vis/loop_closure_cloud - Landmarks visible in last loop closure
visual_slam/vis/pose_graph_nodes - Pose graph nodes
visual_slam/vis/pose_graph_edges - Pose graph edges

Saving the map

Naturally, we would like to save the stored landmarks and pose graph in a map. We have implemented a ROS2 action to save the map to the disk called SaveMap.

Loading and Localization in the Map

Once the map has been saved to the disk, it can be used later to localize the robot. To load the map into the memory, we have made a ROS2 action called LoadMapAndLocalize. It requires a map file path and a prior(initial guess of where the robot is in the map) pose. Given the prior pose and current set of camera frames, Elbrus tries to find the pose of the landmarks in the requested map that matches the current set. If the the localization is successful, it will load the map in the memory and, if it fails, it will continue building a new map.

Both SaveMap and LoadMapAndLocalize can take some time to complete. Hence, they are designed to be asynchronous to avoid interfering with odometry calculations.

System Requirements

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

Jetson

x86_64

Ubuntu 20.04+
CUDA 11.4 supported discrete GPU
Nvidia driver version >= 470.103.01

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.1 (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"

Then restart the Docker service:

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 set up a new workspace in the container.

Docker build notes

You will need to reference the runtime nvidia container on the docker run command. Also for apriltags it is required to pass the device name to the docker container. Additionally it is easier to bind a host workspace with all the ros packages (including isaac ros repos). I have had to add the cuda libs as binds due to them not being included in the docker file. Here is an example docker run command:

'sudo docker run --runtime nvidia --rm --privileged --net=host -v /tmp/.X11-unix/:/tmp/.X11-unix/ -v /home/dirksavage/workspaces/isaac_ros-dev/:/workspaces/isaac_ros-dev/ -v /usr/local/cuda-10.2/targets/aarch64-linux/lib/:/usr/local/cuda-10.2/targets/aarch64-linux/lib/ -v /tmp/argus_socket:/tmp/argus_socket -v /usr/src/jetson_multimedia_api/:/usr/src/jetson_multimedia_api/ --device /dev/video0 -e DISPLAY=$DISPLAY -it apriltag_accelerated:v2 /bin/bash'

notice jetson_multimedia bind. You will need to install it on host with 'sudo apt install nvidia-l4t-jetson-multimedia-api'

I don't remember why argus socket bind is required, but without it I think running the argus camera node failed. Here is an example to run argus with a raspberry pi cam:

'ros2 run isaac_ros_argus_camera_mono isaac_ros_argus_camera_mono --ros-args -r /image_raw:=/image_rect -p device:=0 -p sensor:=4 -p output_encoding:=mono8 -p camera_info_url:=file:///workspaces/isaac_ros-dev/ros_ws/isaac_ros_apriltag/config/rpi_cam.yaml'

Another dependency is nvidia-cudnn8. This is needed for the camera_calibration node. On the host install with 'sudo apt install nvidia-cudnn8', and the cuda libs should be found by binding the libs workspaces in the docker run example above
In your binded ros2 workspace, you may find target export name errors, see the following for a fix: 'cd ros_ws' 'sudo chown -R admin:admin .'
If you get cmake rosidl_generate_interfaces errors, you may need to switch which python version via update alternatives:

solved by setting update-alternatives "sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.6 2"

"sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1"

"sudo update-alternatives --config python"

For VSLAM if you have colcon build errors with vlsam or nvengine interfaces, you need to change an underlay regex in ament_cmake_export_targets-extras.cmake.in > this file is in /opt/ros/foxy/install/share/ament_cmake_export_targets/cmake

change line 12 to: set(_regex "foreach$(_cmake)?expected?[Tt]arget (IN ITEMS )?(.+)$") change line 21 to: string(REGEX REPLACE "${_regex}" "\3" _targets "${_foreach_targets}")'

reference: https://github.com/ament/ament_cmake/commit/11c44dbd646846a2e8ca912155f0704e9b0c3c57

Package Dependencies

Note: isaac_ros_common is used for running tests and/or creating a development container, and isaac_ros_image_pipeline is used as an executable dependency in launch files.

Coordinate Frames

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

input_base_frame: The name of the frame used to calculate transformation between baselink and left camera. The default value is empty(''), which means the value of base_frame_ will be used. If input_base_frame_ and base_frame_ are both empty, the left camera is assumed to be in the robot's center.
input_left_camera_frame: The frame associated with left eye of the stereo camera. Note that this is not the same as the optical frame. The default value is empty(''), which means the left camera is in the robot's center and left_pose_right will be calculated from the CameraInfo message.
input_right_camera_frame: The frame associated with the right eye of the stereo camera. Note that this is not the same as the optical frame. The default value is empty(''), which means left and right cameras have identity rotation and are horizontally aligned, so left_pose_right will be calculated from CameraInfo.
input_imu_frame: The frame associated with the IMU sensor (if available). It is used to calculate left_pose_imu.

Quickstart

Building the Package

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 and its dependencies. 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_slam &&
git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline &&
git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common

Install package dependencies in the ROS workspace. In our case it is your_ws

rosdep install -i -r --from-paths src --rosdistro foxy -y --skip-keys "libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv"

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.

Working with a 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_slam using the launch files:

ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py

In a separate terminal, spin up RViz with a default configuration file to see the rich visualizations.
```
rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/default.cfg.rviz
```

Try moving your camera, and you should see something like this:
To see the odometry messages, in a separate terminal echo the contents of the /visual_slam/tracking/odometry topic with the following command:
```
ros2 topic echo /visual_slam/tracking/odometry
```

Working with Isaac Sim

Ensure you have Isaac Sim set up correctly and choose the appropriate working environment [Native/Docker&Cloud]. This walkthrough uses the native workstation setup for Isaac Sim.
Set up the Nucleus server as shown here to access the assets to work with Isaac Sim.
Open an instance of Isaac Sim from Omniverse Launcher and use the Isaac Sim App Selector to start it.
Set up the Isaac Sim ROS2 bridge as described here.
Open up the Isaac ROS common USD scene located at:

omniverse://<your_nucleus_server>/Isaac/Samples/ROS/Scenario/carter_warehouse_apriltags_worker.usd.

Wait for it to load completly.
Enable the right camera for a stereo image pair. Go to the stage tab and select /World/Carter_ROS/ROS_Camera_Stereo_Right, then tick the enabled checkbox.
Press Play to start publishing data from the Isaac Sim application.

In a separate terminal, start isaac_ros_visual_slam using the launch files:

ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_isaac_sim.launch.py

In a separate terminal, send the signal to rotate the robot about its own axis 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 RViz with default configuration file to see the rich visualizations as the robot moves.
```
rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/default.cfg.rviz
```
To see the odometry messages, in a separate terminal echo the contents of the /visual_slam/tracking/odometry topic with the following command:
```
ros2 topic echo /visual_slam/tracking/odometry
```

Saving and using the map

As soon as you start the visual SLAM node, it starts storing the landmarks and the pose graph. You can save them in a map and store the map onto a disk. Make a call to the SaveMap ROS2 Action with the following command:

ros2 action send_goal /visual_slam/save_map isaac_ros_visual_slam_interfaces/action/SaveMap "{map_url: /path/to/save/the/map}"

Now, you will try to load and localize in the previously saved map. First, stop the visual_slam node lauched for creating and saving the map, then relaunch it.

Use the following command to load the map from the disk and provide an approximate start location(prior):

ros2 action send_goal /visual_slam/load_map_and_localize isaac_ros_visual_slam_interfaces/action/LoadMapAndLocalize "{map_url: /path/to/save/the/map localize_near_point: {x: x_val, y: y_val, z: z_val}}"

Once the above step returns success, you have successfully loaded and localized your robot in the map. If it results in failure, there might be a possibilty that the current landmarks from the approximate start location are not matching with stored landmarks and you need to provide another valid value.

Working with Isaac Sim with Hardware in the loop (HIL)

The following instructions are for a setup where we can run the sample on a Jetson device and Isaac Sim on an x86 machine. These instructions will use the ROS_DOMAIN_ID environment variable to have a separate logical network for Isaac Sim and the sample application.

Note: Before executing any of the ROS commands, make sure to set the ROS_DOMAIN_ID variable first.

Complete steps 1 and 2 of Working with Isaac Sim section.
Open the location of the Isaac Sim package in the terminal by clicking the Open in Terminal button in the Isaac Sim App Selector window.
In the terminal opened by the previous step, set the ROS_DOMAIN_ID as shown:
```
export ROS_DOMAIN_ID=<some_number>
```
Launch Isaac Sim from the script as shown:
```
./isaac-sim.sh
```
Continue with step 4 of the Working with Isaac Sim section. Ensure the ROS_DOMAIN_ID variable is set before running the sample application. Note: In this case, step 8 of the Working with Isaac Sim section must be performed on a Jetson device (i.e. launching the isaac_ros_visual_slam package on the "hardware").

Package Reference