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>

corelib/src/CMakeLists.txt

@@ -101,6 +101,7 @@ SET(SRC_FILES
     odometry/OdometryVINSFusion.cpp
     odometry/OdometryOpenVINS.cpp
     odometry/OdometryOpen3D.cpp
+	odometry/OdometryCuVSLAM.cpp
     
     IMU.cpp
     IMUThread.cpp
@@ -764,6 +765,13 @@ IF(ORB_SLAM_FOUND)
 	)
 ENDIF(ORB_SLAM_FOUND)
 
+IF(CUVSLAM_FOUND)
+    SET(LIBRARIES
+		${LIBRARIES}
+		cuvslam::cuvslam
+	)
+ENDIF(CUVSLAM_FOUND)
+
 IF(GTSAM_FOUND)
 	# Make sure GTSAM is built with system Eigen, not the included one in its package
 	IF(GTSAM_INCLUDE_DIR)

corelib/src/Odometry.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/odometry/OdometryOpen3D.h"
+#include "rtabmap/core/odometry/OdometryCuVSLAM.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -112,6 +113,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOpen3D:
 		odometry = new OdometryOpen3D(parameters);
 		break;
+	case Odometry::kTypeCuVSLAM:
+		odometry = new OdometryCuVSLAM(parameters);
+		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);