Adding OpenCV's GPU GFTT/ OpticalFlow and CudaSift support (#1330)

* Adding GFTT and SIFT Cuda support

* Working CudaSift

* Disable cudasift option when not available

* Added check to avoid re-allocating gpu memory everytime parseParameters is called. Added workaround of to detect/ignore invalid descriptors

* Added SIFT/PreciseUpscale and SIFT/Upscale parameters. Adjusted max octave to behave more like opencv

* Refactored how maximum features are thresholded, to be more similar to OpenCV version

* Updated loop closure benchmark scripts

* Cuda optical flow tmp commit

* Added Stereo/Gpu Vis/CorFlowGpu parameters (optical flow gpu integration for F2F odom and stereo correspondences)

* Fixed build without opencv cuda

* Fixed build with Opencv 4.10

* ZED: updated parameters to match zed sdk 4

* MRPT requires C++17

* updated max octave limit CudaSift

corelib/include/rtabmap/core/Features2d.h

@@ -45,6 +45,7 @@ namespace gpu {
 	class SURF_GPU;
 	class ORB_GPU;
 	class FAST_GPU;
+	class GoodFeaturesToTrackDetector_GPU;
 }
 }
 typedef cv::SIFT CV_SIFT;
@@ -57,13 +58,14 @@ typedef cv::BRISK CV_BRISK;
 typedef cv::gpu::SURF_GPU CV_SURF_GPU;
 typedef cv::gpu::ORB_GPU CV_ORB_GPU;
 typedef cv::gpu::FAST_GPU CV_FAST_GPU;
+typedef cv::gpu::GoodFeaturesToTrackDetector_GPU CV_GFTT_GPU;
 #else
 namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class DAISY;
 class BriefDescriptorExtractor;
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
 #endif
 class SURF;
@@ -72,9 +74,10 @@ namespace cuda {
 class FastFeatureDetector;
 class ORB;
 class SURF_CUDA;
+class CornersDetector;
 }
 }
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
 #else
 typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
@@ -90,8 +93,12 @@ typedef cv::ORB CV_ORB;
 typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
 typedef cv::cuda::ORB CV_ORB_GPU;
 typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
+typedef cv::cuda::CornersDetector CV_GFTT_GPU;
 #endif
 
+// CudaSift fork: https://github.com/matlabbe/CudaSift
+class SiftData;
+
 namespace rtabmap {
 
 class ORBextractor;
@@ -294,9 +301,18 @@ private:
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool preciseUpscale_;
 	bool rootSIFT_;
+	bool gpu_;
+	float guaussianThreshold_;
+	bool upscale_;
 
-	cv::Ptr<CV_SIFT> _sift;
+	cv::Ptr<CV_SIFT> sift_;
+	SiftData * cudaSiftData_;
+	float * cudaSiftMemory_;
+	cv::Size cudaSiftMemorySize_;
+	cv::Mat cudaSiftDescriptors_;
+	bool cudaSiftUpscaling_;
 };
 
 //ORB
@@ -425,8 +441,10 @@ private:
 	int _blockSize;
 	bool _useHarrisDetector;
 	double _k;
+	bool _gpu;
 
 	cv::Ptr<CV_GFTT> _gftt;
+	cv::Ptr<CV_GFTT_GPU> _gpuGftt;
 };
 
 //GFTT_BRIEF

corelib/include/rtabmap/core/Parameters.h

@@ -281,12 +281,15 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SURF, GpuVersion,        bool, false,  "GPU-SURF: Use GPU version of SURF. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
     RTABMAP_PARAM(SURF, GpuKeypointsRatio,  float, 0.01, "Used with SURF GPU.");
 
-    RTABMAP_PARAM(SIFT, NFeatures,         int, 0,       "The number of best features to retain. The features are ranked by their scores (measured in SIFT algorithm as the local contrast).");
-    RTABMAP_PARAM(SIFT, NOctaveLayers,     int, 3,       "The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution.");
-    RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, "The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, NOctaveLayers,     int, 3,       "The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution. Not used by CudaSift, the number of octaves is still computed automatically.");
+    RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, uFormat("The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift (see %s instead).", kSIFTGaussianThreshold().c_str()));
     RTABMAP_PARAM(SIFT, EdgeThreshold,     double, 10,   "The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
     RTABMAP_PARAM(SIFT, Sigma,             double, 1.6,  "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
-    RTABMAP_PARAM(SIFT, RootSIFT,          bool,  false, "Apply RootSIFT normalization of the descriptors.");
+    RTABMAP_PARAM(SIFT, PreciseUpscale,    bool, false,  "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
+    RTABMAP_PARAM(SIFT, RootSIFT,          bool, false,  "Apply RootSIFT normalization of the descriptors.");
+    RTABMAP_PARAM(SIFT, Gpu,               bool, false,  "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
+    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, Upscale,           bool, false,  "CudaSift: Whether to enable upscaling.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
 
@@ -305,6 +308,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GFTT, BlockSize,         int, 3,       "");
     RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
     RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");
+    RTABMAP_PARAM(GFTT, Gpu,               bool, false,  "GPU-GFTT: Use GPU version of GFTT. This option is enabled only if OpenCV>=3 is built with CUDA and GPUs are detected.");
 
     RTABMAP_PARAM(ORB, ScaleFactor,   float, 2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
     RTABMAP_PARAM(ORB, NLevels,       int, 3,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
@@ -714,6 +718,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
@@ -791,6 +796,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
 
     RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
 

corelib/include/rtabmap/core/RegistrationVis.h

@@ -91,6 +91,7 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
+	bool _flowGpu;
 	float _nndr;
 	int _nnType;
 	bool _gmsWithRotation;

corelib/include/rtabmap/core/SensorData.h

@@ -314,6 +314,13 @@ public:
 
 	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
 
+#ifdef HAVE_OPENCV_CUDEV
+	const cv::cuda::GpuMat & imageRawGpu() const {return _imageRawGpu;}
+	void setImageRawGpu(const cv::cuda::GpuMat & image) {_imageRawGpu = image;}
+	const cv::cuda::GpuMat & depthOrRightRawGpu() const {return _depthOrRightRawGpu;}
+	void setDepthOrRightRawGpu(const cv::cuda::GpuMat & image) {_depthOrRightRawGpu = image;}
+#endif
+
 private:
 	int _id;
 	double _stamp;
@@ -365,6 +372,14 @@ private:
 	GPS gps_;
 
 	IMU imu_;
+
+#ifdef HAVE_OPENCV_CUDEV
+	// Temporary buffers used for some optimizations,
+	// particulary to avoid host<->device copies if same 
+	// data are re-used
+	cv::cuda::GpuMat _imageRawGpu;
+	cv::cuda::GpuMat _depthOrRightRawGpu;
+#endif
 };
 
 }

corelib/include/rtabmap/core/Stereo.h

@@ -49,6 +49,13 @@ public:
 			const cv::Mat & rightImage,
 			const std::vector<cv::Point2f> & leftCorners,
 			std::vector<unsigned char> & status) const;
+#ifdef HAVE_OPENCV_CUDEV
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::cuda::GpuMat & leftImage,
+			const cv::cuda::GpuMat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+#endif
 
 	cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
 	int iterations() const   {return iterations_;}
@@ -56,6 +63,7 @@ public:
 	float minDisparity() const {return minDisparity_;}
 	float maxDisparity() const {return maxDisparity_;}
 	bool winSSD() const      {return winSSD_;}
+	virtual bool isGpuEnabled() const {return false;}
 
 private:
 	int winWidth_;
@@ -78,11 +86,27 @@ public:
 			const cv::Mat & rightImage,
 			const std::vector<cv::Point2f> & leftCorners,
 			std::vector<unsigned char> & status) const;
+	
+#ifdef HAVE_OPENCV_CUDEV
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::cuda::GpuMat & leftImage,
+			const cv::cuda::GpuMat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+#endif
 
 	float epsilon() const {return epsilon_;}
+	virtual bool isGpuEnabled() const;
+
+private:
+	void updateStatus(
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		std::vector<unsigned char> & status) const;
 
 private:
 	float epsilon_;
+	bool gpu_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/camera/CameraStereoZed.h

@@ -49,7 +49,7 @@ public:
 public:
 	CameraStereoZed(
 			int deviceId,
-			int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO
+			int resolution = -1, // -1 = AUTO, 0=HD4K 1=HD2K 2=HD1080 3=HD1200 4=HD720 5=SVGA 6=VGA
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,

corelib/src/CMakeLists.txt

@@ -539,6 +539,14 @@ IF(PDAL_FOUND)
 	ENDIF(PDAL_VERSION VERSION_LESS "1.7")
 ENDIF(PDAL_FOUND)
 
+IF(CudaSift_FOUND)
+	#target
+	SET(LIBRARIES
+		${LIBRARIES}
+		cudasift
+	)
+ENDIF(CudaSift_FOUND)
+
 
 IF(loam_velodyne_FOUND)
 	SET(INCLUDE_DIRS 

corelib/src/Features2d.cpp

@@ -75,11 +75,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
   #include <opencv2/cudafeatures2d.hpp>
 #endif
+#ifdef HAVE_OPENCV_CUDAIMGPROC
+#include <opencv2/cudaimgproc.hpp>
+#endif
 
 #ifdef RTABMAP_FASTCV
 #include <fastcv.h>
 #endif
 
+#ifdef RTABMAP_CUDASIFT
+#include <cudasift/cudaImage.h>
+#include <cudasift/cudaSift.h>
+#endif
 namespace rtabmap {
 
 void Feature2D::filterKeypointsByDepth(
@@ -329,7 +336,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		}
 		else
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with the hessian threshold", keypoints.size());
 			// Remove words under the new hessian threshold
 
 			// Sort words by hessian
@@ -858,15 +865,36 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 			data.stereoCameraModels()[0].isValidForProjection())
 		{
 			//stereo
-			cv::Mat imageMono;
-			// convert to grayscale
-			if(data.imageRaw().channels() > 1)
+			cv::Mat imageLeft = data.imageRaw();
+			cv::Mat imageRight = data.rightRaw();
+#ifdef HAVE_OPENCV_CUDEV
+			cv::cuda::GpuMat d_imageLeft;
+			cv::cuda::GpuMat d_imageRight;
+			if(_stereo->isGpuEnabled())
 			{
-				cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
+				d_imageLeft = data.imageRawGpu();
+				if(d_imageLeft.empty()) {
+					d_imageLeft = cv::cuda::GpuMat(imageLeft);
+				}
+				// convert to grayscale
+				if(d_imageLeft.channels() > 1) {
+					cv::cuda::GpuMat tmp;
+					cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY);
+					d_imageLeft = tmp;
+				}
+				d_imageRight = data.depthOrRightRawGpu();
+				if(d_imageRight.empty()) {
+					d_imageRight = cv::cuda::GpuMat(imageRight);
+				}
 			}
 			else
+#endif
 			{
-				imageMono = data.imageRaw();
+				// convert to grayscale (right image should be already grayscale)
+				if(imageLeft.channels() > 1)
+				{
+					cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
+				}
 			}
 
 			std::vector<cv::Point2f> leftCorners;
@@ -877,11 +905,24 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 			if(data.stereoCameraModels().size() == 1)
 			{
 				std::vector<unsigned char> status;
-				rightCorners = _stereo->computeCorrespondences(
-						imageMono,
-						data.rightRaw(),
-						leftCorners,
-						status);
+#ifdef HAVE_OPENCV_CUDEV
+				if(_stereo->isGpuEnabled())
+				{
+					rightCorners = _stereo->computeCorrespondences(
+							d_imageLeft,
+							d_imageRight,
+							leftCorners,
+							status);
+				}
+				else
+#endif
+				{
+					rightCorners = _stereo->computeCorrespondences(
+							imageLeft,
+							imageRight,
+							leftCorners,
+							status);
+				}
 
 				if(ULogger::level() >= ULogger::kWarning)
 				{
@@ -916,8 +957,8 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 			}
 			else
 			{
-				int subImageWith = imageMono.cols / data.stereoCameraModels().size();
-				UASSERT(imageMono.cols % subImageWith == 0);
+				int subImageWith = imageLeft.cols / data.stereoCameraModels().size();
+				UASSERT(imageLeft.cols % subImageWith == 0);
 				std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
 				std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
 				// Assign keypoints per camera
@@ -937,11 +978,24 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 					if(!subLeftCorners[i].empty())
 					{
 						std::vector<unsigned char> status;
-						rightCorners = _stereo->computeCorrespondences(
-								imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
-								data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+#ifdef HAVE_OPENCV_CUDEV
+						if(_stereo->isGpuEnabled())
+						{
+							rightCorners = _stereo->computeCorrespondences(
+								d_imageLeft.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								d_imageRight.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
 								subLeftCorners[i],
 								status);
+						}
+						else
+#endif
+						{
+							rightCorners = _stereo->computeCorrespondences(
+								imageLeft.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								imageRight.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								subLeftCorners[i],
+								status);
+						}
 
 						std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
 								subLeftCorners[i],
@@ -1143,13 +1197,29 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
 	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
 	sigma_(Parameters::defaultSIFTSigma()),
-	rootSIFT_(Parameters::defaultSIFTRootSIFT())
+	preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
+	rootSIFT_(Parameters::defaultSIFTRootSIFT()),
+	gpu_(Parameters::defaultSIFTGpu()),
+	guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	upscale_(Parameters::defaultSIFTUpscale()),
+	cudaSiftData_(0),
+	cudaSiftMemory_(0),
+	cudaSiftUpscaling_(upscale_)
 {
 	parseParameters(parameters);
 }
 
 SIFT::~SIFT()
 {
+#ifdef RTABMAP_CUDASIFT
+	if(cudaSiftData_) {
+		FreeSiftData(*cudaSiftData_);
+		delete cudaSiftData_;
+	}
+	if(cudaSiftMemory_) {
+		FreeSiftTempMemory(cudaSiftMemory_);
+	}
+#endif
 }
 
 void SIFT::parseParameters(const ParametersMap & parameters)
@@ -1160,21 +1230,45 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
+	Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
 	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
-
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+	Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
+	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
+	
+	if(gpu_)
+	{
+#ifdef RTABMAP_CUDASIFT
+		UDEBUG("Init SiftData");
+		if(cudaSiftData_ == 0) {
+			cudaSiftData_ = new SiftData();
+			InitSiftData(*cudaSiftData_, 8192, true, true);
+		}
+#else
+		UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
+		gpu_ = false;
+#endif
+	}
+	
+	if(!gpu_)
+	{
+		#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
-	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
+		sift_ = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
 #else
-	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+		sift_ = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
 #endif
 #else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+		UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
+#elif CV_MAJOR_VERSION>4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION>=8)// >=4.8
+		sift_ = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_, preciseUpscale_);
 #else // >=4.4, >=3.4.11
-	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+		sift_ = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
 #endif
+	}
+
 }
 
 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
@@ -1182,35 +1276,149 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat imgRoi(image, roi);
-	cv::Mat maskRoi;
-	if(!mask.empty())
+#ifdef RTABMAP_CUDASIFT
+	if(gpu_)
 	{
-		maskRoi = cv::Mat(mask, roi);
+		/* Read image using OpenCV and convert to floating point. */
+		int w = imgRoi.cols;
+		int h = imgRoi.rows;
+		cv::Mat img_h;
+		imgRoi.convertTo(img_h, CV_32FC1);
+		CudaImage img_d;
+		img_d.Allocate(w, h, iAlignUp(w, 128), false, NULL, (float*)img_h.data);
+		img_d.Download();
+
+		// Compute number of octaves like OpenCV based on resolution
+		// ref: https://github.com/opencv/opencv/blob/4d665419992dda6e40364f741ae4765176b64bb0/modules/features2d/src/sift.dispatch.cpp#L538
+		// *** stack smashing detected *** if "-2" term is higher
+		int numOctaves = cvRound(std::log( (double)std::min(w*(upscale_?2:1), h*(upscale_?2:1)) ) / std::log(2.) - (upscale_?3:2));
+		if(numOctaves < 1) {
+			numOctaves = 1;
+		}
+		else if (numOctaves>7)
+		{
+			numOctaves = 7; // hard-coded limit in CudaSift
+		}
+		float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
+		float thresh = guaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
+		float edgeLimit = edgeThreshold_;   
+		float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
+		UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
+
+		if(cudaSiftMemory_ && (cudaSiftMemorySize_ != cv::Size(w, h) || cudaSiftUpscaling_ != upscale_)) {
+			// Resolution changed, reset buffer
+			FreeSiftTempMemory(cudaSiftMemory_);
+			cudaSiftMemory_ = 0;
+		}
+
+		if(cudaSiftMemory_ == 0) {
+			cudaSiftMemory_ = AllocSiftTempMemory(w, h, numOctaves, upscale_);
+			UASSERT(cudaSiftMemory_ != 0);
+			cudaSiftMemorySize_ = cv::Size(w, h);
+			cudaSiftUpscaling_ = upscale_;
+		}
+
+		ExtractSift(*cudaSiftData_, img_d, numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_, cudaSiftMemory_);
+		UDEBUG("%d features extracted", cudaSiftData_->numPts);
+		
+		// Convert CudaSift into OpenCV format
+		cudaSiftDescriptors_ = cv::Mat();
+		if(cudaSiftData_->numPts)
+		{
+			int maxKeypoints = this->getMaxFeatures();
+			if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
+			{
+				maxKeypoints = cudaSiftData_->numPts;
+			}
+
+			// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
+			// Sort words by hessian
+			std::multimap<float, int> hessianMap; // <hessian,id>
+			for(int i=0; i<cudaSiftData_->numPts; ++i)
+			{
+				// Ignore keypoints with invalid descriptors
+				float *desc = cudaSiftData_->h_data[i].data;
+				if(desc[0] != 0 && desc[0] == desc[63] && desc[0] == desc[127])
+				{
+					//UWARN("Invalid decsriptor? skipping: %f,%f,%f", cudaSiftData_->h_data[i].xpos, cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].scale);
+					//std::cout << cv::Mat(1, 128*4, CV_8UC1, desc) << std::endl;
+					continue;
+				}
+				//Keep track of the data, to be easier to manage the data in the next step
+				hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
+			}
+
+			if((int)hessianMap.size() < maxKeypoints)
+			{
+				maxKeypoints = hessianMap.size();
+			}
+
+			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+			keypoints.resize(maxKeypoints);
+			cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
+			for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
+			{
+				int i = iter->second;
+				float *desc = cudaSiftData_->h_data[i].data;
+				cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
+				keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
+				keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
+				keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
+				keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
+				keypoints[k].response = cudaSiftData_->h_data[i].sharpness; 
+				keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
+			}
+		}
 	}
+	else
+#endif
+	{
+		cv::Mat maskRoi;
+		if(!mask.empty())
+		{
+			maskRoi = cv::Mat(mask, roi);
+		}
+
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
-	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+		sift_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+		UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 #else // >=4.4, >=3.4.11
-	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+		sift_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #endif
+	}
 	return keypoints;
 }
 
 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
+#ifdef RTABMAP_CUDASIFT
+	if(gpu_)
+	{
+		if((int)keypoints.size() == cudaSiftDescriptors_.rows)
+		{
+			return cudaSiftDescriptors_.clone();
+		}
+		else
+		{
+			UERROR("CudaSift: keypoints size %ld is not equal to extracted descriptors size %d", keypoints.size(), cudaSiftDescriptors_.rows);
+			return cv::Mat();
+		}
+	}
+#endif
+
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
-	_sift->compute(image, keypoints, descriptors);
+	sift_->compute(image, keypoints, descriptors);
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 #else // >=4.4, >=3.4.11
-	_sift->compute(image, keypoints, descriptors);
+	sift_->compute(image, keypoints, descriptors);
 #endif
 	if( rootSIFT_ && !descriptors.empty())
 	{
@@ -1790,7 +1998,8 @@ GFTT::GFTT(const ParametersMap & parameters) :
 		_minDistance(Parameters::defaultGFTTMinDistance()),
 		_blockSize(Parameters::defaultGFTTBlockSize()),
 		_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
-		_k(Parameters::defaultGFTTK())
+		_k(Parameters::defaultGFTTK()),
+		_gpu(Parameters::defaultGFTTGpu())
 {
 	parseParameters(parameters);
 }
@@ -1808,12 +2017,45 @@ void GFTT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
 	Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
 	Parameters::parse(parameters, Parameters::kGFTTK(), _k);
+	Parameters::parse(parameters, Parameters::kGFTTGpu(), _gpu);
 
 #if CV_MAJOR_VERSION < 3
-	_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
-#else
-	_gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+	if(_gpu)
+	{
+		UWARN("GPU version of GFTT is not implemented for OpenCV<3! Using CPU version instead...");
+		_gpu = false;
+	}
 #endif
+
+#ifdef HAVE_OPENCV_CUDAIMGPROC
+	if(_gpu && cv::cuda::getCudaEnabledDeviceCount() == 0)
+	{
+		UWARN("GPU version of GFTT not available! Using CPU version instead...");
+		_gpu = false;
+	}
+#else
+	if(_gpu)
+	{
+		UWARN("GPU version of GFTT not available (OpenCV cudaimageproc module)! Using CPU version instead...");
+		_gpu = false;
+	}
+#endif
+	if(_gpu)
+	{
+#ifdef HAVE_OPENCV_CUDAIMGPROC
+		_gpuGftt = cv::cuda::createGoodFeaturesToTrackDetector(CV_8UC1, this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#else
+		UFATAL("not supposed to be here!");
+#endif
+	}
+	else
+	{
+#if CV_MAJOR_VERSION < 3
+		_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
+#else
+		_gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#endif
+	}
 }
 
 std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
@@ -1826,7 +2068,25 @@ std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, con
 	{
 		maskRoi = cv::Mat(mask, roi);
 	}
-	_gftt->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+
+#if CV_MAJOR_VERSION >= 3 && defined(HAVE_OPENCV_CUDAIMGPROC)
+	if(_gpu)
+	{
+		cv::cuda::GpuMat imgGpu(imgRoi);
+		cv::cuda::GpuMat maskGpu(maskRoi);
+		cv::cuda::GpuMat cornersGpu;
+		_gpuGftt->detect(imgGpu, cornersGpu, maskGpu);
+		std::vector<cv::Point2f> corners(cornersGpu.cols);
+		cv::Mat cornersMat(1, cornersGpu.cols, CV_32FC2, (void*)&corners[0]);
+		cornersGpu.download(cornersMat);
+		cv::KeyPoint::convert(corners, keypoints, _blockSize);
+	}
+	else
+#endif
+	{
+		_gftt->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+	}
+	
 	return keypoints;
 }
 

corelib/src/Parameters.cpp

@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.21.7
+		removedParameters_.insert(std::make_pair("SIFT/NFeatures",    std::make_pair(false, "")));
+
 		// 0.21.3
 		removedParameters_.insert(std::make_pair("GridGlobal/FullUpdate",    std::make_pair(false, "")));
 
@@ -682,6 +685,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With CudaSift:";
+#ifdef RTABMAP_CUDASIFT
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With TORO:";
 #ifdef RTABMAP_TORO

corelib/src/RegistrationVis.cpp

@@ -49,6 +49,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/xfeatures2d.hpp> // For GMS matcher
 #endif
 
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+#include <opencv2/cudaoptflow.hpp>
+#include <opencv2/cudaimgproc.hpp>
+#endif
+
 #include <rtflann/flann.hpp>
 
 
@@ -79,6 +84,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
+		_flowGpu(Parameters::defaultVisCorFlowGpu()),
 		_nndr(Parameters::defaultVisCorNNDR()),
 		_nnType(Parameters::defaultVisCorNNType()),
 		_gmsWithRotation(Parameters::defaultGMSWithRotation()),
@@ -139,6 +145,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
 	Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
+	Parameters::parse(parameters, Parameters::kVisCorFlowGpu(), _flowGpu);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
 	Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
 	Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
@@ -160,6 +167,14 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
 	UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
 
+#ifndef HAVE_OPENCV_CUDAOPTFLOW
+	if(_flowGpu)
+	{
+		UERROR("%s is enabled but RTAB-Map is not built with OpenCV CUDA, disabling it.", Parameters::kVisCorFlowGpu().c_str());
+		_flowGpu = false;
+	}
+#endif
+
 	if(_nnType == 6)
 	{
 		// verify that we have Python3 support
@@ -467,18 +482,59 @@ Transform RegistrationVis::computeTransformationImpl(
 		if(_correspondencesApproach == 1) //Optical Flow
 		{
 			UDEBUG("");
-			// convert to grayscale
-			if(imageFrom.channels() > 1)
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+			cv::cuda::GpuMat d_imageFrom;
+			cv::cuda::GpuMat d_imageTo;
+			if (_flowGpu)
 			{
-				cv::Mat tmp;
-				cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
-				imageFrom = tmp;
+				UDEBUG("GPU optical flow: preparing GPU image data...");
+				d_imageFrom = fromSignature.sensorData().imageRawGpu();
+				if(d_imageFrom.empty() && !imageFrom.empty()) {
+					d_imageFrom = cv::cuda::GpuMat(imageFrom);
+				}
+				// convert to grayscale
+				if(d_imageFrom.channels() > 1) {
+					cv::cuda::GpuMat tmp;
+					cv::cuda::cvtColor(d_imageFrom, tmp, cv::COLOR_BGR2GRAY);
+					d_imageFrom = tmp;
+				}
+				if(fromSignature.sensorData().imageRawGpu().empty())
+				{
+					fromSignature.sensorData().setImageRawGpu(d_imageFrom); // buffer it
+				}
+				
+				d_imageTo = toSignature.sensorData().imageRawGpu();
+				if(d_imageTo.empty() && !imageTo.empty()) {
+					d_imageTo = cv::cuda::GpuMat(imageTo);
+				}
+				// convert to grayscale
+				if(d_imageTo.channels() > 1) {
+					cv::cuda::GpuMat tmp;
+					cv::cuda::cvtColor(d_imageTo, tmp, cv::COLOR_BGR2GRAY);
+					d_imageTo = tmp;
+				}
+				if(toSignature.sensorData().imageRawGpu().empty())
+				{
+					toSignature.sensorData().setImageRawGpu(d_imageTo); // buffer it
+				}
+				UDEBUG("GPU optical flow: preparing GPU image data... done!");
 			}
-			if(imageTo.channels() > 1)
+			else
+#endif
 			{
-				cv::Mat tmp;
-				cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
-				imageTo = tmp;
+				// convert to grayscale
+				if(imageFrom.channels() > 1)
+				{
+					cv::Mat tmp;
+					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
+					imageFrom = tmp;
+				}
+				if(imageTo.channels() > 1)
+				{
+					cv::Mat tmp;
+					cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
+					imageTo = tmp;
+				}
 			}
 
 			std::vector<cv::Point3f> kptsFrom3D;
@@ -568,9 +624,38 @@ Transform RegistrationVis::computeTransformationImpl(
 				// Find features in the new left image
 				UDEBUG("guessSet = %d", guessSet?1:0);
 				std::vector<unsigned char> status;
-				std::vector<float> err;
-				UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-				cv::calcOpticalFlowPyrLK(
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+				if (_flowGpu)
+				{
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
+					cv::cuda::GpuMat d_cornersFrom(cornersFrom);
+					cv::cuda::GpuMat d_cornersTo(cornersTo);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device end");
+					cv::cuda::GpuMat d_status;
+					cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(
+						cv::Size(_flowWinSize, _flowWinSize), guessSet ? 0 : _flowMaxLevel, _flowIterations, guessSet);
+
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc begin");
+					d_pyrLK_sparse->calc(d_imageFrom, d_imageTo, d_cornersFrom, d_cornersTo, d_status);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc end");
+
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host begin");
+					// Transfer back data to CPU
+					cornersTo = std::vector<cv::Point2f>(d_cornersTo.cols);
+					cv::Mat matCornersTo(1, d_cornersTo.cols, CV_32FC2, (void*)&cornersTo[0]);
+					d_cornersTo.download(matCornersTo);
+
+					status = std::vector<unsigned char>(d_status.cols);
+					cv::Mat matStatus(1, d_status.cols, CV_8UC1, (void*)&status[0]);
+					d_status.download(matStatus);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host end");
+				}
+				else
+#endif
+				{
+					std::vector<float> err;
+					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+					cv::calcOpticalFlowPyrLK(
 						imageFrom,
 						imageTo,
 						cornersFrom,
@@ -578,10 +663,11 @@ Transform RegistrationVis::computeTransformationImpl(
 						status,
 						err,
 						cv::Size(_flowWinSize, _flowWinSize),
-						guessSet?0:_flowMaxLevel,
-						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
-						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
-				UDEBUG("cv::calcOpticalFlowPyrLK() end");
+						guessSet ? 0 : _flowMaxLevel,
+						cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4);
+					UDEBUG("cv::calcOpticalFlowPyrLK() end");
+				}
 
 				UASSERT(kptsFrom.size() == kptsFrom3D.size());
 				std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());

corelib/src/Stereo.cpp

@@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <opencv2/video/tracking.hpp>
 
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+#include <opencv2/cudaoptflow.hpp>
+#endif
+
 namespace rtabmap {
 
 Stereo * Stereo::create(const ParametersMap & parameters)
@@ -92,9 +96,22 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 	return rightCorners;
 }
 
+#ifdef HAVE_OPENCV_CUDEV
+std::vector<cv::Point2f> Stereo::computeCorrespondences(
+		const cv::cuda::GpuMat & leftImage,
+		const cv::cuda::GpuMat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	UERROR("GPU support for this approach is not implemented!");
+	return std::vector<cv::Point2f>();
+}
+#endif
+
 StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
 		Stereo(parameters),
-		epsilon_(Parameters::defaultStereoEps())
+		epsilon_(Parameters::defaultStereoEps()),
+		gpu_(Parameters::defaultStereoGpu())
 {
 	this->parseParameters(parameters);
 }
@@ -103,8 +120,24 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
 {
 	Stereo::parseParameters(parameters);
 	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
+	Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
+#ifndef HAVE_OPENCV_CUDAOPTFLOW
+	if(gpu_)
+	{
+		UERROR("%s is enabled but RTAB-Map is not built with OpenCV CUDA, disabling it.", Parameters::kStereoGpu().c_str());
+		gpu_ = false;
+	}
+#endif
 }
 
+bool StereoOpticalFlow::isGpuEnabled() const
+{
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+	return gpu_;
+#else
+	return false;
+#endif
+}
 
 std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		const cv::Mat & leftImage,
@@ -113,20 +146,83 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		std::vector<unsigned char> & status) const
 {
 	std::vector<cv::Point2f> rightCorners;
-	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
 	std::vector<float> err;
-	util2d::calcOpticalFlowPyrLKStereo(
-			leftImage,
-			rightImage,
-			leftCorners,
-			rightCorners,
-			status,
-			err,
-			this->winSize(),
-			this->maxLevel(),
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
-			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
-	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+	if(gpu_)
+	{
+		cv::cuda::GpuMat d_leftImage(leftImage);
+		cv::cuda::GpuMat d_rightImage(rightImage);
+		return computeCorrespondences(d_leftImage, d_rightImage, leftCorners, status);
+	}
+	else
+#endif
+	{
+		UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
+		util2d::calcOpticalFlowPyrLKStereo(
+				leftImage,
+				rightImage,
+				leftCorners,
+				rightCorners,
+				status,
+				err,
+				this->winSize(),
+				this->maxLevel(),
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+		UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
+	}
+	updateStatus(leftCorners, rightCorners, status);
+	return rightCorners;
+}
+
+#ifdef HAVE_OPENCV_CUDEV
+std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
+		const cv::cuda::GpuMat & leftImage,
+		const cv::cuda::GpuMat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	std::vector<cv::Point2f> rightCorners;
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
+	cv::cuda::GpuMat d_leftImage(leftImage);
+	cv::cuda::GpuMat d_rightImage(rightImage);
+	cv::cuda::GpuMat d_leftCorners(leftCorners);
+	cv::cuda::GpuMat d_rightCorners;
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device end");
+	cv::cuda::GpuMat d_status;
+	cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(
+		this->winSize(), this->maxLevel(), this->iterations());
+
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc begin");
+	d_pyrLK_sparse->calc(d_leftImage, d_rightImage, d_leftCorners, d_rightCorners, d_status);
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc end");
+
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host begin");
+	// Transfer back data to CPU
+	rightCorners = std::vector<cv::Point2f>(d_rightCorners.cols);
+	cv::Mat matRightCorners(1, d_rightCorners.cols, CV_32FC2, (void*)&rightCorners[0]);
+	d_rightCorners.download(matRightCorners);
+
+	status = std::vector<unsigned char>(d_status.cols);
+	cv::Mat matStatus(1, d_status.cols, CV_8UC1, (void*)&status[0]);
+	d_status.download(matStatus);
+	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host end");
+
+	updateStatus(leftCorners, rightCorners, status);
+
+#else
+	UERROR("GPU support for this approach is not implemented!");
+#endif
+	return rightCorners;
+}
+#endif
+
+void StereoOpticalFlow::updateStatus(
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		std::vector<unsigned char> & status) const
+{
 	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
 	int countFlowRejected = 0;
 	int countDisparityRejected = 0;
@@ -147,8 +243,6 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		}
 	}
 	UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
-
-	return rightCorners;
 }
 
 } /* namespace rtabmap */

corelib/src/camera/CameraStereoZed.cpp

@@ -286,15 +286,29 @@ CameraStereoZed::CameraStereoZed(
 	UDEBUG("");
 #ifdef RTABMAP_ZED
 #if ZED_SDK_MAJOR_VERSION < 4
-	if(resolution_ == 3)
+	if(resolution_ == 1 || resolution_ == 2) // HD2K, HD1080
 	{
-		resolution_ = 2;
+		resolution_ -= 1; // HD2K=0, HD1080=1
 	}
-	else if(resolution_ == 5)
+	if(resolution_ == 3) // HD1200
 	{
-		resolution_ = 3;
+		resolution_ = 1; // HD1080=1
+	}
+	if(resolution_ == 4 || resolution_ == -1)
+	{
+		resolution_ = 2; // HD720=2
+	}
+	else if(resolution_ == 5 || resolution_ == 6) // SVGA, VGA
+	{
+		resolution_ = 3; // VGA=3
+	}
+#else // ZED=4
+	if(resolution_ == -1)
+	{
+		resolution_ = int(sl::RESOLUTION::AUTO); // AUTO
 	}
 #endif
+
 #if ZED_SDK_MAJOR_VERSION < 3
 	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
 	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -336,7 +350,13 @@ CameraStereoZed::CameraStereoZed(
 	src_(CameraVideo::kVideoFile),
 	usbDevice_(0),
 	svoFilePath_(filePath),
-	resolution_(2),
+#if ZED_SDK_MAJOR_VERSION < 3
+	resolution_(sl::RESOLUTION_HD720),
+#elif ZED_SDK_MAJOR_VERSION < 4
+	resolution_(sl::RESOLUTION::HD720),
+#else
+	resolution_(int(sl::RESOLUTION::AUTO)),
+#endif
 	quality_(quality),
 	selfCalibration_(selfCalibration),
 	sensingMode_(sensingMode),