Isaac ROS 3.0.0

Author: hemalshahNV

.gitattributes

@@ -22,4 +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
+isaac_ros_visual_slam/test/test_cases/rosbags/** filter=lfs diff=lfs merge=lfs -text

README.md

@@ -1,6 +1,7 @@
 # Isaac ROS Visual SLAM
 
-Hardware-accelerated, simultaneous localization and mapping (SLAM) using stereo visual inertial odometry (SVIO).
+NVIDIA-accelerated, simultaneous localization and mapping (SLAM) using stereo
+visual inertial odometry (SVIO).
 
 <div align="center"><a class="reference internal image-reference" href="https://media.githubusercontent.com/media/NVIDIA-ISAAC-ROS/.github/main/resources/isaac_ros_docs/repositories_and_packages/isaac_ros_visual_slam/cuvslam_ros_3.gif/"><img alt="image" src="https://media.githubusercontent.com/media/NVIDIA-ISAAC-ROS/.github/main/resources/isaac_ros_docs/repositories_and_packages/isaac_ros_visual_slam/cuvslam_ros_3.gif/" width="800px"/></a></div>
 
@@ -16,32 +17,32 @@ Jetson](https://gateway.on24.com/wcc/experience/elitenvidiabrill/1407606/3998202
 
 ## Overview
 
-[Isaac ROS Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) provides a high-performance, best-in-class ROS 2 package
+[Isaac ROS Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam)
+provides a high-performance, best-in-class ROS 2 package
 for VSLAM (visual simultaneous localization and mapping). This package
-uses a stereo camera with an IMU to estimate odometry as an input to
-navigation. It is GPU accelerated to provide real-time, low-latency
-results in a robotics application. VSLAM provides an additional odometry
-source for mobile robots (ground based) and can be the primary odometry
-source for drones.
+uses one or more stereo cameras and optionally an IMU to estimate
+odometry as an input to navigation. It is GPU accelerated to provide
+real-time, low-latency results in a robotics application. VSLAM
+provides an additional odometry source for mobile robots
+(ground based) and can be the primary odometry source for drones.
 
 VSLAM provides a method for visually estimating the position of a robot
 relative to its start position, known as VO (visual odometry). This is
 particularly useful in environments where GPS is not available (such as
 indoors) or intermittent (such as urban locations with structures
 blocking line of sight to GPS satellites). This method is designed to
-use left and right stereo camera frames and an IMU (inertial measurement
-unit) as input. It uses input stereo image pairs to find matching key
-points in the left and right images; using the baseline between the left
-and right camera, it can estimate the distance to the key point. Using
-two consecutive input stereo image pairs, VSLAM can track the 3D motion
-of key points between the two consecutive images to estimate the 3D
-motion of the camera-which is then used to compute odometry as an output
+use multiple stereo camera frames and an IMU (inertial measurement
+unit) as input. It uses stereo image pairs to find matching key
+points. Using the baseline between the camera pairs, it can estimate
+the distance to the key point. Using consecutive images, VSLAM
+can track the motion of key points to estimate the 3D motion of the
+camera-which is then used to compute odometry as an output
 to navigation. Compared to the classic approach to VSLAM, this method
 uses GPU acceleration to find and match more key points in real-time,
 with fine tuning to minimize overall reprojection error.
 
-Key points depend on distinctive features in the left and right camera
-image that can be repeatedly detected with changes in size, orientation,
+Key points depend on distinctive features in the images
+that can be repeatedly detected with changes in size, orientation,
 perspective, lighting, and image noise. In some instances, the number of
 key points may be limited or entirely absent; for example, if the camera
 field of view is only looking at a large solid colored wall, no key
@@ -100,15 +101,19 @@ outdoor scenes.
 
 ## Performance
 
-| Sample Graph<br/><br/>                                                                                                                          | Input Size<br/><br/>     | AGX Orin<br/><br/>                                                                                                                                          | Orin NX<br/><br/>                                                                                                                                          | Orin Nano 8GB<br/><br/>                                                                                                                                      | x86_64 w/ RTX 4060 Ti<br/><br/>                                                                                                                               |
-|-------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| [Visual SLAM Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts/isaac_ros_visual_slam_node.py)<br/><br/><br/><br/> | 720p<br/><br/><br/><br/> | [228 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-agx_orin.json)<br/><br/><br/>40 ms<br/><br/> | [127 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-orin_nx.json)<br/><br/><br/>74 ms<br/><br/> | [113 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-orin_nano.json)<br/><br/><br/>65 ms<br/><br/> | [456 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-nuc_4060ti.json)<br/><br/><br/>37 ms<br/><br/> |
+| Sample Graph<br/><br/>                                                                                                                                                                                                           | Input Size<br/><br/>      | Nova Carter<br/><br/>                                                                                                                                   |
+|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [Multicam Visual SLAM Live Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_perceptor_nova_benchmark/scripts/isaac_ros_visual_slam_graph.py)<br/><br/><br/>4 Hawk Cameras<br/><br/> | 1200p<br/><br/><br/><br/> | [30.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_4_hawk_vslam_graph-carter_v2.4.json)<br/><br/><br/><br/> |
+
+> [!Note]
+> This benchmark can only be run on a [Nova Orin](https://developer.nvidia.com/isaac/nova-orin) compatible system.
 
 ---
 
 ## Documentation
 
-Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_visual_slam/index.html) to learn how to use this repository.
+Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_visual_slam/index.html) to learn how to use
+this repository.
 
 ---
 
@@ -124,4 +129,4 @@ Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/re
 
 ## Latest
 
-Update 2023-10-18: Improved stability.
+Update 2024-05-30: Add support for multi-cam VIO

isaac_ros_visual_slam/CMakeLists.txt

@@ -1,5 +1,5 @@
 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-# Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -45,24 +45,19 @@ endforeach()
 
 include_directories(${CUVSLAM}/include)
 add_library(cuvslam SHARED IMPORTED)
-if( ${ARCHITECTURE} STREQUAL "x86_64" )
-  set_property(TARGET cuvslam PROPERTY IMPORTED_LOCATION ${CUVSLAM}/lib_x86_64_cuda_11_8/libcuvslam.so)
-elseif( ${ARCHITECTURE} STREQUAL "aarch64" )
-  set_property(TARGET cuvslam PROPERTY IMPORTED_LOCATION ${CUVSLAM}/lib_aarch64_jetpack51/libcuvslam.so)
-endif()
+set_property(TARGET cuvslam PROPERTY IMPORTED_LOCATION ${CUVSLAM}/lib/libcuvslam.so)
 
 # visual_slam_node
 ament_auto_add_library(
   visual_slam_node SHARED
   src/visual_slam_node.cpp
-  src/impl/cuvslam_ros_convertion.cpp
+  src/impl/cuvslam_ros_conversion.cpp
   src/impl/landmarks_vis_helper.cpp
   src/impl/localizer_vis_helper.cpp
   src/impl/pose_cache.cpp
   src/impl/posegraph_vis_helper.cpp
   src/impl/visual_slam_impl.cpp
   src/impl/viz_helper.cpp
-  src/impl/qos.cpp
 )
 target_link_libraries(visual_slam_node cuvslam)
 rclcpp_components_register_nodes(visual_slam_node "nvidia::isaac_ros::visual_slam::VisualSlamNode")
@@ -75,27 +70,30 @@ ament_auto_add_executable(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME} visual_slam_node)
 
 # Install cuVSLAM
-if( ${ARCHITECTURE} STREQUAL "x86_64" )
-  install(DIRECTORY ${CUVSLAM}/lib_x86_64_cuda_11_8/ DESTINATION share/cuvslam)
-elseif( ${ARCHITECTURE} STREQUAL "aarch64" )
-  install(DIRECTORY ${CUVSLAM}/lib_aarch64_jetpack51/ DESTINATION share/cuvslam)
-endif()
-set_target_properties(visual_slam_node PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE)
-set_target_properties(visual_slam_node PROPERTIES INSTALL_RPATH "$ORIGIN/../share/cuvslam")
+install(DIRECTORY ${CUVSLAM}/lib DESTINATION share/cuvslam)
+
+set_target_properties(visual_slam_node PROPERTIES
+  BUILD_WITH_INSTALL_RPATH TRUE
+  BUILD_RPATH_USE_ORIGIN TRUE
+  INSTALL_RPATH_USE_LINK_PATH TRUE
+  INSTALL_RPATH "$ORIGIN/../share/cuvslam")
+
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-
   # The FindPythonInterp and FindPythonLibs modules are removed
   if(POLICY CMP0148)
     cmake_policy(SET CMP0148 OLD)
   endif()
 
   find_package(launch_testing_ament_cmake REQUIRED)
-  add_launch_test(test/isaac_ros_visual_slam_pol.py)
-  add_launch_test(test/isaac_ros_visual_slam_srv.py)
+  add_launch_test(test/isaac_ros_visual_slam_pol_single_cam_imu.py)
+  add_launch_test(test/isaac_ros_visual_slam_pol_multi_cam_imu.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_reset.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_get_all_poses.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_set_slam_pose.py)
 
   find_package(ament_cmake_gtest REQUIRED)
   ament_add_gtest(${PROJECT_NAME}_test_message_buffer test/test_message_buffer.cpp)
@@ -115,6 +113,18 @@ if(BUILD_TESTING)
   ament_target_dependencies(${PROJECT_NAME}_test_message_stream_sequencer
     std_msgs
   )
+
+  ament_add_gtest(${PROJECT_NAME}_test_message_stream_synchronizer
+    test/test_message_stream_synchronizer.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test_message_stream_synchronizer PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test_message_stream_synchronizer
+    rclcpp
+    geometry_msgs
+  )
 endif()
 
-ament_auto_package(INSTALL_TO_SHARE config launch params)
+ament_auto_package(INSTALL_TO_SHARE config launch rviz)

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/cuvslam_ros_conversion.hpp

@@ -0,0 +1,95 @@
+// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef ISAAC_ROS_VISUAL_SLAM__IMPL__CUVSLAM_ROS_CONVERSION_HPP_
+#define ISAAC_ROS_VISUAL_SLAM__IMPL__CUVSLAM_ROS_CONVERSION_HPP_
+
+#include <string>
+
+#include "cuvslam.h" // NOLINT - include .h without directory
+#include "cv_bridge/cv_bridge.h"
+#include "isaac_ros_visual_slam/impl/types.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "eigen3/Eigen/Eigen"
+
+namespace nvidia
+{
+namespace isaac_ros
+{
+namespace visual_slam
+{
+
+// Transformation converting from
+// Canonical ROS Frame (x-forward, y-left, z-up) to
+// cuVSLAM Frame       (x-right, y-up, z-backward)
+// ROS    ->  cuVSLAM
+//  x     ->    -z
+//  y     ->    -x
+//  z     ->     y
+const tf2::Transform cuvslam_pose_canonical(tf2::Matrix3x3(
+    0, -1, 0,
+    0, 0, 1,
+    -1, 0, 0
+));
+
+// Transformation converting from
+// cuVSLAM Frame       (x-right, y-up, z-backward) to
+// Canonical ROS Frame (x-forward, y-left, z-up)
+const tf2::Transform canonical_pose_cuvslam(cuvslam_pose_canonical.inverse());
+
+// Transformation converting from
+// Optical Frame    (x-right, y-down, z-forward) to
+// cuVSLAM Frame    (x-right, y-up, z-backward)
+// Optical   ->  cuVSLAM
+//    x      ->     x
+//    y      ->    -y
+//    z      ->    -z
+const tf2::Transform cuvslam_pose_optical(tf2::Matrix3x3(
+    1, 0, 0,
+    0, -1, 0,
+    0, 0, -1
+));
+
+// Transformation converting from
+// cuVSLAM Frame    (x-right, y-up, z-backward) to
+// Optical Frame    (x-right, y-down, z-forward)
+const tf2::Transform optical_pose_cuvslam(cuvslam_pose_optical.inverse());
+
+tf2::Transform ChangeBasis(
+  const tf2::Transform & target_pose_source, const tf2::Transform & source_pose_source);
+
+CUVSLAM_Pose TocuVSLAMPose(const tf2::Transform & tf_mat);
+tf2::Transform FromcuVSLAMPose(const CUVSLAM_Pose & cuvslam_pose);
+
+CUVSLAM_ImageEncoding TocuVSLAMImageEncoding(const std::string & image_encoding);
+
+CUVSLAM_Image TocuVSLAMImage(
+  int32_t camera_index, const ImageType & image_view, const int64_t & acqtime_ns);
+
+CUVSLAM_ImuMeasurement TocuVSLAMImuMeasurement(const ImuType::ConstSharedPtr & msg_imu);
+
+void FillIntrinsics(const CameraInfoType::ConstSharedPtr & msg, CUVSLAM_Camera & camera);
+
+void FillExtrinsics(const tf2::Transform & base_pose_camera_optical, CUVSLAM_Camera & camera);
+
+Eigen::Matrix<float, 6, 6> FromcuVSLAMCovariance(float covariance[36]);
+
+}  // namespace visual_slam
+}  // namespace isaac_ros
+}  // namespace nvidia
+
+#endif  // ISAAC_ROS_VISUAL_SLAM__IMPL__CUVSLAM_ROS_CONVERSION_HPP_

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/cuvslam_ros_convertion.hpp

@@ -1,11 +1,11 @@
 // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/has_subscribers.hpp

@@ -1,11 +1,11 @@
 // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/landmarks_vis_helper.hpp

@@ -1,11 +1,11 @@
 // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/limited_vector.hpp

@@ -1,11 +1,11 @@
 // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
-// Copyright (c) 2021-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,