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
2026-09-02 17:40:23 +08:00 · 2024-09-13 14:22:54 -07:00
parent f3ccfcb452
commit 69ac21f811
37 changed files with 2253 additions and 1375 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/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 
--- a/corelib/src/Features2d.cpp
+++ b/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;
 }

--- a/corelib/src/Parameters.cpp
+++ b/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
--- a/corelib/src/RegistrationVis.cpp
+++ b/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());
--- a/corelib/src/Stereo.cpp
+++ b/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 */
--- a/corelib/src/camera/CameraStereoZed.cpp
+++ b/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),