CuVSLAM VO Strategy (#1583)

* integrating cuvslam, stereo cam initialization

* fixing transformations

* use isaac ros TocuVSLAMPose

* organizing

* handling covariance conversion

* cleaning up config

* moving cmake instructions to FindCuVSLAM.cmake file

* fixing ui integration

* updating software license for cuvslam

* improving memory management

* cleaning up header more

* reverting mistake in GUI

* using opaque pointers for header definitions, compiles

* back to typed definitions using forward-referencing

* fixing dangling pointer

* cleaning up

* fixing variable scope issues

* Using cuda to allocate gpu memory

* fixing transfrom from cuvslam back to ros coordinate frame

* reducing excessive logs

* removing redundant if true in cuvslam cmake

* moving find cuda into cuvslam cmake

* cleaning cmake to use modern target

* adding copyright line

* Finding tf2 and eigen3 in cmake

* simplifying clean up logic

* better logs

* moving cuvslam implementation methods into cpp

* fixing build with implmentation function declarations moved to the cpp

* cleaning transformation logic and adding grayscale support

* typo

* removing unecessary class member variable for processed images

* remove trailing underscore from initialize cuvslam function signature

* locally scoping config params since they are copied by cuvslam

* removing unecessary member variables and using locally scoped values since cuvslam seems to copy them

* more cleaning and refactoring

* fixing cmake inconsistancies and surpress warning

* fixing up baseline transform

* trying to remove tf2 for transformations

* fixing incorect baseline transform swap

* comment, begin reset logic investigation

* better handling of previous pose and transform during error cases

* hunting for reset bug, not found

* observation export implementatin, currently getting strange errors

* multi cam support

* Memory fixes

* comments

* make cuda stream a member variable so it isn't initialized and destroyed on every frame

* preparing to add ground constraints

* adding ground constraints, working

* adding wheel odom fusion into cuvslam tracker

* parsing parameter to apply planar constraints

* removing planar constraint in config, doesn't do anything

* using proper cuda stream type for member variable

* cleaning up for merge

* only parse params when cuvslam is compiled

* updating error message if using rgb-d instead of stereo

* removing wheel odom testing logs

* reverting bgr to rgb conversion

* fixing initial pose logic

* return null transform on invalid covariance

* debugging covariance

* guard against low velocity situations where cuvslam covariance spikes, but we aren't lost

* cleaning things up

---------

Co-authored-by: Felix Toft <felix@robust.ai>
Co-authored-by: matlabbe <matlabbe@gmail.com>

cmake_modules/FindCuVSLAM.cmake

@@ -0,0 +1,79 @@
+# - Find cuVSLAM library (https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam)
+#
+#  CUVSLAM_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  CUVSLAM_FOUND         - Set to false, or undefined, if cuVSLAM isn't found.
+#  CUVSLAM_INCLUDE_DIRS  - The cuVSLAM include directory.
+#  CUVSLAM_LIBRARIES     - The cuVSLAM library to link against.
+
+find_package(CUDA REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+find_path(CUVSLAM_INCLUDE_DIRS 
+    NAMES cuvslam.h
+    PATHS
+        /usr/include
+        /usr/local/include
+        /opt/cuvslam/include
+        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/include
+        $ENV{CUVSLAM_ROOT}/include
+        $ENV{CUVSLAM_ROOT_DIR}/include
+)
+
+find_library(CUVSLAM_LIBRARY 
+    NAMES cuvslam
+    PATHS
+        /usr/lib
+        /usr/local/lib
+        /opt/cuvslam/lib
+        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib
+        $ENV{CUVSLAM_ROOT}/lib
+        $ENV{CUVSLAM_ROOT_DIR}/lib
+)
+
+if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY)
+    set(CUVSLAM_FOUND TRUE)
+    set(CUVSLAM_LIBRARIES 
+        ${CUVSLAM_LIBRARY} 
+        ${CUDA_LIBRARIES}
+        # Eigen3 is header-only, so we don't need to link to it
+    )
+    set(CUVSLAM_INCLUDE_DIRS 
+        ${CUVSLAM_INCLUDE_DIRS} 
+        ${CUDA_INCLUDE_DIRS} 
+        ${EIGEN3_INCLUDE_DIR}
+    )
+endif()
+
+# Handle the QUIET and REQUIRED arguments
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CuVSLAM
+    FOUND_VAR CUVSLAM_FOUND
+    REQUIRED_VARS CUVSLAM_LIBRARIES CUVSLAM_INCLUDE_DIRS
+    HANDLE_COMPONENTS
+)
+
+if(CUVSLAM_FOUND)
+    # Create imported target for modern CMake usage
+    if(NOT TARGET cuvslam::cuvslam)
+        add_library(cuvslam::cuvslam UNKNOWN IMPORTED)
+        set_target_properties(cuvslam::cuvslam PROPERTIES
+            IMPORTED_LOCATION "${CUVSLAM_LIBRARY}"
+            INTERFACE_INCLUDE_DIRECTORIES "${CUVSLAM_INCLUDE_DIRS}"
+            INTERFACE_LINK_LIBRARIES "${CUVSLAM_LIBRARIES};Eigen3::Eigen"
+        )
+    endif()
+    
+    # Show which cuVSLAM was found only if not quiet
+    if(NOT CUVSLAM_FIND_QUIETLY)
+        message(STATUS "Found cuVSLAM: ${CUVSLAM_LIBRARIES}")
+    endif()
+else()
+    # Fatal error if cuVSLAM is required but not found
+    if(CUVSLAM_FIND_REQUIRED)
+        message(FATAL_ERROR "Could not find cuVSLAM library")
+    endif()
+endif()
+
+mark_as_advanced(CUVSLAM_INCLUDE_DIRS CUVSLAM_LIBRARY)