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/include/rtabmap/core/Odometry.h

@@ -56,7 +56,8 @@ public:
 		kTypeVINSFusion = 9,
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
-		kTypeOpen3D = 12
+		kTypeOpen3D = 12,
+		kTypeCuVSLAM = 13
 	};
 
 public:

corelib/include/rtabmap/core/Parameters.h

@@ -456,7 +456,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");

corelib/include/rtabmap/core/odometry/OdometryCuVSLAM.h

@@ -0,0 +1,86 @@
+/*
+Copyright (c) 2025 Felix Toft
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYCUVSLAM_H_
+#define ODOMETRYCUVSLAM_H_
+
+#include <rtabmap/core/Odometry.h>
+#include <memory>
+
+#ifdef RTABMAP_CUVSLAM
+#include <cuvslam.h>
+#include <ground_constraint.h>
+#include <cuda_runtime.h>
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryCuVSLAM : public Odometry
+{
+public:
+	OdometryCuVSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryCuVSLAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeCuVSLAM;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_CUVSLAM
+	CUVSLAM_TrackerHandle cuvslam_handle_;
+	CUVSLAM_GroundConstraintHandle ground_constraint_handle_;
+
+	std::vector<CUVSLAM_Camera> cuvslam_cameras_;
+	std::vector<std::array<float, 12>> intrinsics_;
+	
+	// State tracking
+	bool initialized_;
+	bool lost_;
+	bool tracking_;
+	bool planar_constraints_;
+	Transform previous_pose_;
+	double last_timestamp_;
+
+	//visualization
+	std::vector<CUVSLAM_Observation> observations_;
+	std::vector<CUVSLAM_Landmark> landmarks_;
+	
+	// GPU memory management
+	std::vector<uint8_t *> gpu_left_image_data_; // pointers to all gpu images
+	std::vector<uint8_t *> gpu_right_image_data_;
+	std::vector<size_t> gpu_left_image_sizes_; // size of one image
+	std::vector<size_t> gpu_right_image_sizes_;
+	cudaStream_t cuda_stream_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYCUVSLAM_H_ */

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);