Merge pull request #90 from NVIDIA-ISAAC-ROS/release-dp3.1

Isaac ROS 0.31.0 (DP3.1)

Author: hemalshahNV

.gitattributes

@@ -22,7 +22,4 @@
 # ROS Bags
 **/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text
 **/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text
-isaac_ros_visual_slam/test/test_cases/rosbags/** filter=lfs diff=lfs merge=lfs -text
-
-# Elbrus Library
-isaac_ros_visual_slam/elbrus/** filter=lfs diff=lfs merge=lfs -text
+isaac_ros_visual_slam/test/test_cases/rosbags/** filter=lfs diff=lfs merge=lfs -text

README.md

@@ -1,8 +1,8 @@
-# Elbrus SLAM
+# cuVSLAM
 
-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.
+All SLAM-related operations work in parallel to visual odometry in a separate thread. Input images get copied into GPU and then cuVSLAM starts tracking.
 
-Elbrus uses the following:
+cuVSLAM uses the following:
 
 * 2D features on the input images
 * Landmarks: The patch of pixels in the source images with coordinates of the feature associated with the position of the camera from where that feature is visible.
@@ -12,13 +12,13 @@ Landmarks and PoseGraph are stored in a data structure that doesn't increase its
 
 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 has been seen before. When Elbrus recognizes several landmarks, it determines their position from the data structure.
+During the robot's motion, SLAM stores all landmarks and it continuously verifies if each landmark has been seen before. When cuVSLAM recognizes several landmarks, it determines their position from the data structure.
 
-At this moment, a connection is added to the PoseGraph which makes a loop from the free trail of the poses; this event is termed 'loop closure'. Immediately after that, Elbrus performs a graph optimization that leads to the correction of the current odometric pose and all previous poses in the graph.
+At this moment, a connection is added to the PoseGraph which makes a loop from the free trail of the poses; this event is termed 'loop closure'. Immediately after that, cuVSLAM performs a graph optimization that leads to the correction of the current odometric pose and all previous 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 eventual deletion. This allows you to use Elbrus over a changing terrain.
+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 eventual deletion. This allows you to use cuVSLAM over a changing terrain.
 
-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.
+Along with visual data, cuVSLAM 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.
 
 In case of severe degradation of image input (lights being turned off, dramatic motion blur on a bump while driving, and other possible scenarios), below mentioned motion estimation algorithms will ensure acceptable quality for pose tracking:
 
@@ -41,6 +41,6 @@ Naturally, we would like to save the stored landmarks and pose graph in a map. W
 
 ## 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 ROS 2 action called `LoadMapAndLocalize`. It requires a map file path and a prior pose, which is an initial guess of where the robot is in the map. 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 localization is successful, Elbrus will load the map in the memory. Otherwise, it will continue building a new 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 ROS 2 action called `LoadMapAndLocalize`. It requires a map file path and a prior pose, which is an initial guess of where the robot is in the map. Given the prior pose and current set of camera frames, cuVSLAM tries to find the pose of the landmarks in the requested map that matches the current set. If the localization is successful, cuVSLAM will load the map in the memory. Otherwise, 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.

docs/elbrus-slam.md docs/cuvslam.mddocs/elbrus-slam.md renamed to docs/cuvslam.md

@@ -71,6 +71,7 @@ Last validated with [Isaac Sim 2022.2.1](https://docs.omniverse.nvidia.com/app_i
 
 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` ROS 2 Action with the following command:
 
+> Note: `/path/to/save/the/map` must be a new empty directory everytime you call this action as this action will overwrite the existing contents. 
 ```bash
 ros2 action send_goal /visual_slam/save_map isaac_ros_visual_slam_interfaces/action/SaveMap "{map_url: /path/to/save/the/map}"
 ```

docs/tutorial-isaac-sim.md

@@ -6,7 +6,7 @@
 
 This tutorial walks you through setting up [Isaac ROS Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) with a [Realsense camera](https://www.intel.com/content/www/us/en/architecture-and-technology/realsense-overview.html).
 
-> **Note**: The [launch file](../isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py) provided in this tutorial is designed for a RealSense camera with integrated IMU. If you want to run this tutorial with a RealSense camera without an IMU (like RealSense D435), then change `enable_imu` param in the launch file to `False`.
+> **Note**: The [launch file](../isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py) provided in this tutorial is designed for a RealSense camera with integrated IMU. If you want to run this tutorial with a RealSense camera without an IMU (like RealSense D435), then change `enable_imu_fusion` param in the launch file to `False`.
 <!-- Split blockquote -->
 > **Note**: This tutorial requires a compatible RealSense camera from the list available [here](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/realsense-setup.md#camera-compatibility).
 
@@ -16,7 +16,9 @@ This tutorial walks you through setting up [Isaac ROS Visual SLAM](https://githu
 
 2. Complete the [Quickstart section](../README.md#quickstart) in the main README.
 
-3. \[Terminal 1\] Run `realsense-camera` node and `visual_slam` node
+3. Follow this [page](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model#how-to-obtain-the-parameters-for-your-imu) to obtain [IMU Noise Model](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model) params. Params can be obtained either through datasheet for the IMU or from a ROS package like [this](https://github.com/CruxDevStuff/allan_ros2).
+
+4. \[Terminal 1\] Run `realsense-camera` node and `visual_slam` node
 
    Make sure you have your RealSense camera attached to the system, and then start the Isaac ROS container.
 
@@ -31,21 +33,21 @@ This tutorial walks you through setting up [Isaac ROS Visual SLAM](https://githu
     >   ./scripts/run_dev.sh ${ISAAC_ROS_WS}
     > ```
 
-4. \[Terminal 1\] Inside the container, build and source the workspace:  
+5. \[Terminal 1\] Inside the container, build and source the workspace:  
 
     ```bash
     cd /workspaces/isaac_ros-dev && \
       colcon build --symlink-install && \
       source install/setup.bash
     ```
 
-5. \[Terminal 1\] Run the launch file, which launches the example and wait for 5 seconds:
+6. \[Terminal 1\] Run the launch file, which launches the example and wait for 5 seconds:
 
     ```bash
     ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py
     ```
 
-6. \[Terminal 2\] Attach a second terminal to check the operation.
+7. \[Terminal 2\] Attach a second terminal to check the operation.
 
     Attach another terminal to the running container for issuing other ROS2 commands.
 

docs/tutorial-realsense.md

@@ -0,0 +1,227 @@
+# Validating Isaac ROS Visual SLAM on a Robot
+
+## Overview
+
+This document outlines a procedure that can be used to validate the successful installation and configuration of Isaac ROS Visual SLAM on real hardware.
+
+## Table of Contents
+- [Validating Isaac ROS Visual SLAM on a Robot](#validating-isaac-ros-visual-slam-on-a-robot)
+  - [Overview](#overview)
+  - [Table of Contents](#table-of-contents)
+  - [Prerequisites](#prerequisites)
+  - [Prepare Physical Environment](#prepare-physical-environment)
+  - [Download and Configure Packages](#download-and-configure-packages)
+  - [Build ROS Workspace](#build-ros-workspace)
+  - [Select and Start ROS Visualization Tool](#select-and-start-ros-visualization-tool)
+  - [Run Isaac ROS Visual SLAM and Monitor Results](#run-isaac-ros-visual-slam-and-monitor-results)
+    - [Test 1: Visual Odometry](#test-1-visual-odometry)
+    - [Test 2: Loop Closure](#test-2-loop-closure)
+  - [Next Steps](#next-steps)
+    - [Troubleshooting](#troubleshooting)
+    - [Integrating Isaac ROS Visual SLAM](#integrating-isaac-ros-visual-slam)
+
+## Prerequisites
+
+- [ ] NVIDIA Jetson
+- [ ] Standalone computer with ROS data visualization tool (RViz, Foxglove, etc.)
+- [ ] Compatible stereo camera
+- [ ] Enough space to navigate (~40m loop) 
+- [ ] Tape measure 
+
+
+## Prepare Physical Environment
+
+1.  Examine the designated testing area and identify a single, non-intersecting loop whose path length is approximately 40 meters. This will be the path along which the camera is moved and tracked using Isaac ROS Visual SLAM.
+2.  Define a starting pose along the identified loop. Label this pose with masking tape or a different marker. 
+    
+    One possible approach is to mark an L-shape on the floor, aligning the camera's position and orientation off of the corner of the shape.
+3.  Using a measuring tool, measure out a secondary pose along the loop that is exactly 1 meter along the forward axis from the starting pose. Label this pose in the same manner. 
+
+<div align="center"><img src="../resources/validation_tape.jpg" width="600px"/></div>
+
+## Download and Configure Packages
+
+1. On each of the Jetson and standalone computer, set up your development environment by following the instructions [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md).
+
+    > Note: `${ISAAC_ROS_WS}` is defined to point to either `/ssd/workspaces/isaac_ros-dev/` or `~/workspaces/isaac_ros-dev/`.
+
+2. Set up your stereo camera and connect it to the Jetson.
+  
+   - For HAWK cameras, complete the [HAWK setup tutorial](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/hawk-setup.md).
+   - For RealSense cameras, complete the [RealSense setup tutorial](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/realsense-setup.md).
+
+3. On both devices, clone this repository and its dependencies under `${ISAAC_ROS_WS}/src`.
+
+    ```bash
+    cd ${ISAAC_ROS_WS}/src && \
+    git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam && \
+    git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common && \
+    git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros
+    ```
+
+## Build ROS Workspace
+
+1. On the Jetson, launch the Docker container in a new terminal:
+
+    ```bash
+    cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
+      ./scripts/run_dev.sh ${ISAAC_ROS_WS}
+    ```
+
+2. Inside the container, build and source the workspace:
+
+    ```bash
+    cd /workspaces/isaac_ros-dev && \
+      colcon build --symlink-install && \
+      source install/setup.bash
+    ```
+
+## Select and Start ROS Visualization Tool
+
+1. Ensure that the standalone computer and Jetson device are on the same network.
+2. On the standalone computer, start RViz from the provided configuration file:
+
+    Start the Docker container for RViz on the standalone computer:
+
+    ```bash
+    cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
+      ./scripts/run_dev.sh ${ISAAC_ROS_WS}
+    ```
+
+    > **(Optional)** Configure the `ROS_DOMAIN_ID` environment variable on both devices to eliminate any cross-talk from other ROS nodes on the same network.
+    > 
+    > In each terminal on each device:
+    > ```bash
+    > export ROS_DOMAIN_ID=5 # Any number between 1-101; use the same number for each terminal
+    > ```
+    > **Note**: This environment variable must be set in each terminal, on both the Jetson and the standalone computer. 
+
+    From this terminal, run Rviz2 to display the output:
+
+    ```bash
+    source /workspaces/isaac_ros-dev/install/setup.bash && \
+      rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/default.cfg.rviz 
+    ```
+
+3. On the standalone computer, start logging the estimated transform:
+
+    Attach another terminal to the running container for `tf2_echo`:
+
+    ```bash
+    cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
+      ./scripts/run_dev.sh ${ISAAC_ROS_WS}
+    ```
+
+    From this terminal, run `tf2_echo` to display the transform between the `/map` frame and the appropriate camera frame:
+
+    - HAWK: `/camera`
+    - RealSense: `/camera_link`
+
+    ```bash
+    source /workspaces/isaac_ros-dev/install/setup.bash && \
+      ros2 run tf2_ros tf2_echo map camera_link
+    ```
+
+## Run Isaac ROS Visual SLAM and Monitor Results
+
+### Test 1: Visual Odometry
+
+The purpose of this test is to validate that the system is able to provide accurate measurements of the robot's local motion.
+
+1. Initialize the camera at the marked starting pose. 
+2. On the Jetson, run the Isaac ROS Visual SLAM launch file appropriate for your camera:
+
+    - HAWK:
+      ```bash
+      ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_hawk.launch.py
+      ```
+    - RealSense:
+      ```bash
+      ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py
+      ```
+
+    At this point, the `/map`, `/odom`, and camera frames will all be initialized to the starting pose.
+3. Precisely move the camera straight forward to the marked secondary pose.
+
+4. On the standalone computer and within the `tf2_echo` terminal, note the latest transform reported between `/map` and the appropriate camera frame. 
+
+   <div align="center"><img src="../resources/validation_test1_tf2.png" width="600px"/></div>
+
+   1. Validate Orientation:
+   
+      The orientation error between the measured rotation unit quaternion and the expected rotation unit quaternion (`[0 0 0 1]`) should be less than 15 degrees. This error can be calculated using the process explained [here](https://gitlab.tudelft.nl/-/snippets/190):
+
+      $$q_{measured} = w_m + x_m i + y_m j + z_m k$$
+      $$q_{expected} = w_e + x_e i + y_e j + z_e k$$
+      $$2 \arccos (w_m w_e + x_m x_e + y_m y_e + z_m z_e) \leq 15 \degree $$
+
+   2. Validate Translation:
+
+      The magnitude of the measured translation should be within 5 centimeters of the expected translation (1 meter):
+
+      $$ 0.95 \text{m} \leq \|t_{measured}\| \leq 1.05 \text{m}$$ 
+
+If all real-world measurements correspond to the reported estimates within the expected tolerance, the Visual Odometry test is a sucess. 
+
+### Test 2: Loop Closure
+
+The purpose of this test is to validate that the system is able to appropriately realign the estimated pose path to previously-seen features.
+
+1. Initialize the camera at the marked starting pose. 
+2. On the Jetson, run the Isaac ROS Visual SLAM launch file appropriate for your camera:
+
+    - HAWK:
+      ```bash
+      ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_hawk.launch.py
+      ```
+    - RealSense:
+      ```bash
+      ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py
+      ```
+
+    At this point, the `/map`, `/odom`, and camera frames will all be initialized to the starting pose.
+
+3. Precisely move the camera along the predetermined 40m path.
+
+   > **Note**: To maximize the accuracy of this test, ensure that dynamic obstacles are removed from the robot's field of view. If a nontrivial dynamic obstacle is introduced while the robot is performing the loop, please restart this test.
+
+4. As the camera nears the completion of its first lap, place the camera at rest, taking care to precisely align it with the floor marker indicating the starting pose.
+5. On the standalone computer and within the RViz window, ensure that a nontrivial number of visual features now appear red. Additionally, ensure that the camera frame appears nearly coincident with the `/map` frame.
+
+    > **Note**: It is acceptable for the `/odom` frame to jump around significantly with respect to the `/map` frame. For the purposes of this test, only the transform between the `/map` and camera frames is relevant. For additional information about the design intent behind these frames, please see [REP-0105](https://www.ros.org/reps/rep-0105.html).
+
+    <div align="center"><img src="../resources/validation_test2_rviz.png" width="600px"/></div>
+  
+6. On the standalone computer and within the `tf2_echo` terminal, note the latest transform reported between `/map` and the appropriate camera frame. 
+   <div align="center"><img src="../resources/validation_test2_tf2.png" width="600px"/></div>
+
+   1. Validate Orientation:
+   
+      The orientation error between the measured rotation unit quaternion and the expected rotation unit quaternion (`[0 0 0 1]`, since the path is a closed loop) should be less than 15 degrees. This error can be calculated using the process explained [here](https://gitlab.tudelft.nl/-/snippets/190):
+
+      $$q_{measured} = w_m + x_m i + y_m j + z_m k$$
+      $$q_{expected} = w_e + x_e i + y_e j + z_e k$$
+      $$2 \arccos (w_m w_e + x_m x_e + y_m y_e + z_m z_e) \leq 15 \degree $$
+
+   2. Validate Translation:
+
+      The magnitude of the measured translation should be within 2 meters (5% of the 40m path length) of the expected translation (0 meters, since the path is a closed loop):
+
+      $$ 0 \text{m} \leq \|t_{measured}\| \leq 2 \text{m}$$
+   
+If all real-world measurements correspond to the reported estimates within the expected tolerance, the Loop Closure test is a sucess. 
+
+## Next Steps
+
+### Troubleshooting
+If any step of the above tests did not succeed, then there may be a configuration error with your system. Please follow these [troubleshooting steps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam#troubleshooting).
+
+If the prepared troubleshooting steps are unable to resolve the test failure, please contact your NVIDIA support representative and file an issue report with all relevant details.
+
+### Integrating Isaac ROS Visual SLAM
+
+While the Isaac ROS Visual SLAM node exposes a [multitude of interfaces](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam#isaac_ros_visual_slam), the core way to consume the node's output is through the reported transform between the `/map` and camera frames.
+
+Based on your robot's precise specifications, you can set up a static transform publisher or URDF-based publisher that connects the camera frame to a relevant planning frame on the robot (typically named `/base_link`). Then, your navigation stack can use the transform between `/base_link` and `/map` to plan movements within the local space.
+
+Consider further augmenting your robot's navigation stack with other Isaac ROS packages, such as [Isaac ROS Nvblox](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox) for 3D reconstruction.