Merge branch 'master' of https://github.com/introlab/rtabmap into devel

2026-09-02 01:20:25 +08:00 · 2015-06-18 23:04:14 -04:00
parent 82943e85e8 6fe1e42181
commit b0a20abd66
17 changed files with 450 additions and 253 deletions
--- a/corelib/src/Camera.cpp
+++ b/corelib/src/Camera.cpp
@@ -300,6 +300,7 @@ cv::Mat CameraImages::captureImage()
 					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
 							img.cols, img.rows, img.channels(), img.elemSize(), img.total());

+#if CV_MAJOR_VERSION < 3
 					// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
 					if(img.depth() != CV_8U)
 					{
@@ -309,6 +310,7 @@ cv::Mat CameraImages::captureImage()
 						img = cv::Mat(i, true);
 						cvReleaseImage(&i);
 					}
+#endif

 					if(img.channels()>3)
 					{
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -34,14 +34,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include <opencv2/imgproc/imgproc_c.h>
-#include <opencv2/gpu/gpu.hpp>
 #include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>

-#if RTABMAP_NONFREE == 1
-#if CV_MAJOR_VERSION > 2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-#include <opencv2/nonfree/gpu.hpp>
-#include <opencv2/nonfree/features2d.hpp>
+#if CV_MAJOR_VERSION < 3
+#include <opencv2/gpu/gpu.hpp>
+#else
+#include <opencv2/core/cuda.hpp>
 #endif
+
+#ifdef HAVE_OPENCV_NONFREE
+  #if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >=4
+  #include <opencv2/nonfree/gpu.hpp>
+  #include <opencv2/nonfree/features2d.hpp>
+  #endif
+#endif
+#ifdef HAVE_OPENCV_XFEATURES2D
+  #include <opencv2/xfeatures2d.hpp>
+  #include <opencv2/xfeatures2d/nonfree.hpp>
+  #include <opencv2/xfeatures2d/cuda.hpp>
 #endif

 namespace rtabmap {
@@ -330,12 +341,29 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
 {
-	if(RTABMAP_NONFREE == 0 &&
-	   (type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
+	if(RTABMAP_NONFREE == 0)
 	{
-		UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
-		type = Feature2D::kFeatureOrb;
+		if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
+		{
+#if CV_MAJOR_VERSION < 3
+			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
+#else
+			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
+#endif
+			type = Feature2D::kFeatureOrb;
+		}
+#if CV_MAJOR_VERSION == 3
+		if(type == Feature2D::kFeatureFastBrief ||
+		   type == Feature2D::kFeatureFastFreak ||
+		   type == Feature2D::kFeatureGfttBrief ||
+		   type == Feature2D::kFeatureGfttFreak)
+		{
+			UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
+			type = Feature2D::kFeatureOrb;
+		}
+#endif
 	}
+
 	Feature2D * feature2D = 0;
 	switch(type)
 	{
@@ -432,25 +460,13 @@ SURF::SURF(const ParametersMap & parameters) :
 		extended_(Parameters::defaultSURFExtended()),
 		upright_(Parameters::defaultSURFUpright()),
 		gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
-		gpuVersion_(Parameters::defaultSURFGpuVersion()),
-		_surf(0),
-		_gpuSurf(0)
+		gpuVersion_(Parameters::defaultSURFGpuVersion())
 {
 	parseParameters(parameters);
 }

 SURF::~SURF()
 {
-#if RTABMAP_NONFREE == 1
-	if(_surf)
-	{
-		delete _surf;
-	}
-	if(_gpuSurf)
-	{
-		delete _gpuSurf;
-	}
-#endif
 }

 void SURF::parseParameters(const ParametersMap & parameters)
@@ -466,29 +482,30 @@ void SURF::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);

 #if RTABMAP_NONFREE == 1
-	if(_gpuSurf)
+#if CV_MAJOR_VERSION < 3
+	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuSurf;
-		_gpuSurf = 0;
+		UWARN("GPU version of SURF not available! Using CPU version instead...");
+		gpuVersion_ = false;
 	}
-	if(_surf)
+#else
+	if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _surf;
-		_surf = 0;
+		UWARN("GPU version of SURF not available! Using CPU version instead...");
+		gpuVersion_ = false;
 	}
-
-	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount())
+#endif
+	if(gpuVersion_)
 	{
-		_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_);
+		_gpuSurf = cv::Ptr<CV_SURF_GPU>(new CV_SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_));
 	}
 	else
 	{
-		if(gpuVersion_)
-		{
-			UWARN("GPU version of SURF not available! Using CPU version instead...");
-		}
-
-		_surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
+#if CV_MAJOR_VERSION < 3
+		_surf = cv::Ptr<CV_SURF>(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_));
+#else
+		_surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
+#endif
 	}
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
@@ -502,10 +519,15 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con

 #if RTABMAP_NONFREE == 1
 	cv::Mat imgRoi(image, roi);
-	if(_gpuSurf)
+	if(gpuVersion_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+		cv::cuda::GpuMat imgGpu(imgRoi);
+		(*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints);
+#endif
 	}
 	else
 	{
@@ -522,11 +544,17 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
 #if RTABMAP_NONFREE == 1
-	if(_gpuSurf)
+	if(gpuVersion_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(image);
 		cv::gpu::GpuMat descriptorsGPU;
-		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
+		(*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
+#else
+		cv::cuda::GpuMat imgGpu(image);
+		cv::cuda::GpuMat descriptorsGPU;
+		(*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints, descriptorsGPU, true);
+#endif

 		// Download descriptors
 		if (descriptorsGPU.empty())
@@ -557,22 +585,13 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
 	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
 	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
-	sigma_(Parameters::defaultSIFTSigma()),
-	_sift(0)
+	sigma_(Parameters::defaultSIFTSigma())
 {
 	parseParameters(parameters);
 }

 SIFT::~SIFT()
 {
-#if RTABMAP_NONFREE == 1
-	if(_sift)
-	{
-		delete _sift;
-	}
-#else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
-#endif
 }

 void SIFT::parseParameters(const ParametersMap & parameters)
@@ -586,13 +605,11 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);

 #if RTABMAP_NONFREE == 1
-	if(_sift)
-	{
-		delete _sift;
-		_sift = 0;
-	}
-
-	_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#if CV_MAJOR_VERSION < 3
+	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
+#else
+	_sift = CV_SIFT::create(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#endif
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
@@ -637,23 +654,13 @@ ORB::ORB(const ParametersMap & parameters) :
 		patchSize_(Parameters::defaultORBPatchSize()),
 		gpu_(Parameters::defaultORBGpu()),
 		fastThreshold_(Parameters::defaultFASTThreshold()),
-		nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression()),
-		_orb(0),
-		_gpuOrb(0)
+		nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression())
 {
 	parseParameters(parameters);
 }

 ORB::~ORB()
 {
-	if(_orb)
-	{
-		delete _orb;
-	}
-	if(_gpuOrb)
-	{
-		delete _gpuOrb;
-	}
 }

 void ORB::parseParameters(const ParametersMap & parameters)
@@ -673,29 +680,44 @@ void ORB::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
 	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);

-	if(_gpuOrb)
+#if CV_MAJOR_VERSION < 3
+	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuOrb;
-		_gpuOrb = 0;
+		UWARN("GPU version of ORB not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-	if(_orb)
+#else
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+	if(gpu_)
 	{
-		delete _orb;
-		_orb = 0;
+		UWARN("GPU version of ORB not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
+		gpu_ = false;
 	}
-
-	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
+#endif
+	if(gpu_)
 	{
-		_gpuOrb = new cv::gpu::ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+		UWARN("GPU version of ORB available but not implemented yet! Using CPU version instead...");
+	}
+	gpu_ = false;
+#endif
+	if(gpu_)
+	{
+#if CV_MAJOR_VERSION < 3
+		_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
 		_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
-		if(gpu_)
-		{
-			UWARN("GPU version of ORB not available! Using CPU version instead...");
-		}
-		_orb = new cv::ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+#if CV_MAJOR_VERSION < 3
+		_orb = cv::Ptr<CV_ORB>(new CV_ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
+#else
+		_orb = CV_ORB::create(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+#endif
 	}
 }

@@ -704,10 +726,17 @@ std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, cons
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat imgRoi(image, roi);
-	if(_gpuOrb)
+
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
@@ -726,11 +755,19 @@ cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyP
 		ULOGGER_ERROR("Image is null ?!?");
 		return descriptors;
 	}
-	if(_gpuOrb)
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(image);
 		cv::gpu::GpuMat descriptorsGPU;
-		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
+		(*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
+#else
+		cv::cuda::GpuMat imgGpu(image);
+		cv::cuda::GpuMat descriptorsGPU;
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif

 		// Download descriptors
 		if (descriptorsGPU.empty())
@@ -757,23 +794,13 @@ FAST::FAST(const ParametersMap & parameters) :
 		threshold_(Parameters::defaultFASTThreshold()),
 		nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
 		gpu_(Parameters::defaultFASTGpu()),
-		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
-		_fast(0),
-		_gpuFast(0)
+		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio())
 {
 	parseParameters(parameters);
 }

 FAST::~FAST()
 {
-	if(_fast)
-	{
-		delete _fast;
-	}
-	if(_gpuFast)
-	{
-		delete _gpuFast;
-	}
 }

 void FAST::parseParameters(const ParametersMap & parameters)
@@ -785,28 +812,48 @@ void FAST::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
 	Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);

-	if(_gpuFast)
+#if CV_MAJOR_VERSION < 3
+	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuFast;
-		_gpuFast = 0;
+		UWARN("GPU version of FAST not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-	if(_fast)
+#else
+	if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _fast;
-		_fast = 0;
+		UWARN("GPU version of FAST not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-
-	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+	if(gpu_)
 	{
-		_gpuFast = new cv::gpu::FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
+		UWARN("GPU version of FAST not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
+		gpu_ = false;
+	}
+#endif
+	if(gpu_)
+	{
+		UWARN("GPU version of FAST is available but not yet implemented! Using CPU version instead...");
+	}
+	gpu_ = false;
+#endif
+	if(gpu_)
+	{
+#if CV_MAJOR_VERSION < 3
+		_gpuFast = new CV_FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
-		if(gpu_)
-		{
-			UWARN("GPU version of FAST not available! Using CPU version instead...");
-		}
-		_fast = new cv::FastFeatureDetector(threshold_, nonmaxSuppression_);
+#if CV_MAJOR_VERSION < 3
+		_fast = cv::Ptr<CV_FAST>(new CV_FAST(threshold_, nonmaxSuppression_));
+#else
+		_fast = CV_FAST::create(threshold_, nonmaxSuppression_);
+#endif
 	}
 }

@@ -815,10 +862,16 @@ std::vector<cv::KeyPoint> FAST::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);
-	if(_gpuFast)
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuFast)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuFast.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
@@ -832,18 +885,13 @@ std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, con
 //////////////////////////
 FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
 	FAST(parameters),
-	bytes_(Parameters::defaultBRIEFBytes()),
-	_brief(0)
+	bytes_(Parameters::defaultBRIEFBytes())
 {
 	parseParameters(parameters);
 }

 FAST_BRIEF::~FAST_BRIEF()
 {
-	if(_brief)
-	{
-		delete _brief;
-	}
 }

 void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
@@ -851,19 +899,30 @@ void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
 	FAST::parseParameters(parameters);

 	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
-	if(_brief)
-	{
-		delete _brief;
-		_brief = 0;
-	}
-	_brief = new cv::BriefDescriptorExtractor(bytes_);
+#if CV_MAJOR_VERSION < 3
+	_brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief = CV_BRIEF::create(bytes_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 }

 cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_brief->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 	return descriptors;
 }

@@ -875,18 +934,13 @@ FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
 	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
 	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
 	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves()),
-	_freak(0)
+	nOctaves_(Parameters::defaultFREAKNOctaves())
 {
 	parseParameters(parameters);
 }

 FAST_FREAK::~FAST_FREAK()
 {
-	if(_freak)
-	{
-		delete _freak;
-	}
 }

 void FAST_FREAK::parseParameters(const ParametersMap & parameters)
@@ -898,20 +952,30 @@ void FAST_FREAK::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
 	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);

-	if(_freak)
-	{
-		delete _freak;
-		_freak = 0;
-	}
-
-	_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 }

 cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 	return descriptors;
 }

@@ -924,18 +988,13 @@ GFTT::GFTT(const ParametersMap & parameters) :
 		_minDistance(Parameters::defaultGFTTMinDistance()),
 		_blockSize(Parameters::defaultGFTTBlockSize()),
 		_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
-		_k(Parameters::defaultGFTTK()),
-		_gftt(0)
+		_k(Parameters::defaultGFTTK())
 {
 	parseParameters(parameters);
 }

 GFTT::~GFTT()
 {
-	if(_gftt)
-	{
-		delete _gftt;
-	}
 }

 void GFTT::parseParameters(const ParametersMap & parameters)
@@ -949,12 +1008,11 @@ void GFTT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
 	Parameters::parse(parameters, Parameters::kGFTTK(), _k);

-	if(_gftt)
-	{
-		delete _gftt;
-		_gftt = 0;
-	}
-	_gftt = new cv::GFTTDetector(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#if CV_MAJOR_VERSION < 3
+	_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
+#else
+	_gftt = CV_GFTT::create(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#endif
 }

 std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
@@ -971,18 +1029,13 @@ std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, con
 //////////////////////////
 GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
 	GFTT(parameters),
-	bytes_(Parameters::defaultBRIEFBytes()),
-	_brief(0)
+	bytes_(Parameters::defaultBRIEFBytes())
 {
 	parseParameters(parameters);
 }

 GFTT_BRIEF::~GFTT_BRIEF()
 {
-	if(_brief)
-	{
-		delete _brief;
-	}
 }

 void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
@@ -990,19 +1043,30 @@ void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
 	GFTT::parseParameters(parameters);

 	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
-	if(_brief)
-	{
-		delete _brief;
-		_brief = 0;
-	}
-	_brief = new cv::BriefDescriptorExtractor(bytes_);
+#if CV_MAJOR_VERSION < 3
+	_brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief = CV_BRIEF::create(bytes_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 }

 cv::Mat GFTT_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_brief->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 	return descriptors;
 }

@@ -1014,18 +1078,13 @@ GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
 	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
 	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
 	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves()),
-	_freak(0)
+	nOctaves_(Parameters::defaultFREAKNOctaves())
 {
 	parseParameters(parameters);
 }

 GFTT_FREAK::~GFTT_FREAK()
 {
-	if(_freak)
-	{
-		delete _freak;
-	}
 }

 void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
@@ -1037,20 +1096,30 @@ void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
 	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);

-	if(_freak)
-	{
-		delete _freak;
-		_freak = 0;
-	}
-
-	_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 }

 cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 	return descriptors;
 }

@@ -1060,18 +1129,13 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 BRISK::BRISK(const ParametersMap & parameters) :
 	thresh_(Parameters::defaultBRISKThresh()),
 	octaves_(Parameters::defaultBRISKOctaves()),
-	patternScale_(Parameters::defaultBRISKPatternScale()),
-	brisk_(0)
+	patternScale_(Parameters::defaultBRISKPatternScale())
 {
 	parseParameters(parameters);
 }

 BRISK::~BRISK()
 {
-	if(brisk_)
-	{
-		delete brisk_;
-	}
 }

 void BRISK::parseParameters(const ParametersMap & parameters)
@@ -1082,13 +1146,11 @@ void BRISK::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
 	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);

-	if(brisk_)
-	{
-		delete brisk_;
-		brisk_ = 0;
-	}
-
-	brisk_ = new cv::BRISK(thresh_, octaves_, patternScale_);
+#if CV_MAJOR_VERSION < 3
+	brisk_ = cv::Ptr<CV_BRISK>(new CV_BRISK(thresh_, octaves_, patternScale_));
+#else
+	brisk_ = CV_BRISK::create(thresh_, octaves_, patternScale_);
+#endif
 }

 std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -2128,7 +2128,7 @@ Transform Memory::computeVisualTransform(
 					cameraTransform,
 					100,
 					4.0f,
-					cv::ITERATIVE,
+					0, // cv::SOLVEPNP_ITERATIVE
 					1.0f,
 					0.99f,
 					oldS.getWords3(),
--- a/corelib/src/VWDictionary.cpp
+++ b/corelib/src/VWDictionary.cpp
@@ -34,7 +34,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/utilite/UtiLite.h"

+#include <opencv2/opencv_modules.hpp>
+#if CV_MAJOR_VERSION < 3
 #include <opencv2/gpu/gpu.hpp>
+#else
+#include <opencv2/core/cuda.hpp>
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+#include <opencv2/cudafeatures2d.hpp>
+#endif
+#endif

 #include <fstream>
 #include <string>
@@ -220,11 +228,26 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 	if(strategy!=kNNUndef)
 	{
+#if CV_MAJOR_VERSION < 3
 		if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
 		{
 			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
 			strategy = kNNBruteForce;
 		}
+#else
+		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
+		{
+			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+			strategy = kNNBruteForce;
+		}
+#endif
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+		if(strategy == kNNBruteForceGPU)
+		{
+			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
+			strategy = kNNBruteForce;
+		}
+#endif

 		if(RTABMAP_NONFREE == 0 && strategy == kNNFlannKdTree)
 		{
@@ -466,6 +489,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		else if(_strategy == kNNBruteForceGPU)
 		{
 			bruteForce = true;
+#if CV_MAJOR_VERSION < 3
 			cv::gpu::GpuMat newDescriptorsGpu(descriptors);
 			cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
 			if(type==CV_8U)
@@ -478,6 +502,22 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 				cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
 				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 			}
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+			cv::cuda::GpuMat newDescriptorsGpu(descriptors);
+			cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
+			if(type==CV_8U)
+			{
+				cv::cuda::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
+				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+			}
+			else
+			{
+				cv::cuda::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
+				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+			}
+#endif
+#endif
 		}
 		else
 		{
@@ -681,6 +721,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			else if(_strategy == kNNBruteForceGPU)
 			{
 				bruteForce = true;
+#if CV_MAJOR_VERSION < 3
 				cv::gpu::GpuMat newDescriptorsGpu(query);
 				cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
 				if(type==CV_8U)
@@ -693,6 +734,23 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 					cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
 					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 				}
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+				cv::cuda::GpuMat newDescriptorsGpu(query);
+				cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
+				cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
+				if(type==CV_8U)
+				{
+					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
+					gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				}
+				else
+				{
+					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
+					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				}
+#endif
+#endif
 			}
 			else
 			{
--- a/corelib/src/VisualWord.h
+++ b/corelib/src/VisualWord.h
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

 #include <opencv2/core/core.hpp>
+#include <map>

 namespace rtabmap
 {
--- a/corelib/src/util2d.cpp
+++ b/corelib/src/util2d.cpp
@@ -58,7 +58,8 @@ cv::Mat disparityFromStereoImages(
 	{
 		leftMono = leftImage;
 	}
-
+	cv::Mat disparity;
+#if CV_MAJOR_VERSION < 3
 	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
 	stereo.state->SADWindowSize = 15;
 	stereo.state->minDisparity = 0;
@@ -69,8 +70,20 @@ cv::Mat disparityFromStereoImages(
 	stereo.state->textureThreshold = 10;
 	stereo.state->speckleWindowSize = 100;
 	stereo.state->speckleRange = 4;
-	cv::Mat disparity;
 	stereo(leftMono, rightImage, disparity, CV_16SC1);
+#else
+	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
+	stereo->setBlockSize(15);
+	stereo->setMinDisparity(0);
+	stereo->setNumDisparities(64);
+	stereo->setPreFilterSize(9);
+	stereo->setPreFilterCap(31);
+	stereo->setUniquenessRatio(15);
+	stereo->setTextureThreshold(10);
+	stereo->setSpeckleWindowSize(100);
+	stereo->setSpeckleRange(4);
+	stereo->compute(leftMono, rightImage, disparity);
+#endif
 	return disparity;
 }

--- a/corelib/src/util3d_features.cpp
+++ b/corelib/src/util3d_features.cpp
@@ -521,32 +521,6 @@ std::multimap<int, cv::KeyPoint> aggregate(
 	return words;
 }

-pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
-		const cv::Mat & image,
-		const cv::Mat & imageDepth,
-		float constant,
-		int fastThreshold,
-		bool fastNonmaxSuppression,
-		float maxDepth)
-{
-	// Extract words
-	cv::FastFeatureDetector detector(fastThreshold, fastNonmaxSuppression);
-	std::vector<cv::KeyPoint> kpts;
-	detector.detect(image, kpts);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
-	for(unsigned int i=0; i<kpts.size(); ++i)
-	{
-		pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
-		if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
-		{
-			points->push_back(pt);
-		}
-	}
-	UDEBUG("points %d -> %d", (int)kpts.size(), (int)points->size());
-	return points;
-}
-
 }

 }