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

corelib/include/rtabmap/core/Features2d.h

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef KEYPOINTDESCRIPTOR_H_
-#define KEYPOINTDESCRIPTOR_H_
+#ifndef FEATURES2D_H_
+#define FEATURES2D_H_
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <list>
 #include "rtabmap/core/Parameters.h"
 
+#if CV_MAJOR_VERSION < 3
 namespace cv{
 class SURF;
 class SIFT;
@@ -45,6 +46,44 @@ namespace gpu {
 	class FAST_GPU;
 }
 }
+typedef cv::SIFT CV_SIFT;
+typedef cv::SURF CV_SURF;
+typedef cv::ORB CV_ORB;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::FREAK CV_FREAK;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::BriefDescriptorExtractor CV_BRIEF;
+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;
+#else
+namespace cv{
+namespace xfeatures2d {
+class FREAK;
+class BriefDescriptorExtractor;
+class SIFT;
+class SURF;
+}
+namespace cuda {
+class FastFeatureDetector;
+class ORB;
+class SURF_CUDA;
+}
+}
+typedef cv::xfeatures2d::SIFT CV_SIFT;
+typedef cv::xfeatures2d::SURF CV_SURF;
+typedef cv::ORB CV_ORB;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
+typedef cv::BRISK CV_BRISK;
+typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
+typedef cv::cuda::ORB CV_ORB_GPU;
+typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
+#endif
+
 
 namespace rtabmap {
 
@@ -134,8 +173,8 @@ private:
 	float gpuKeypointsRatio_;
 	bool gpuVersion_;
 
-	cv::SURF * _surf;
-	cv::gpu::SURF_GPU * _gpuSurf;
+	cv::Ptr<CV_SURF> _surf;
+	cv::Ptr<CV_SURF_GPU> _gpuSurf;
 };
 
 //SIFT
@@ -159,7 +198,7 @@ private:
 	double edgeThreshold_;
 	double sigma_;
 
-	cv::SIFT * _sift;
+	cv::Ptr<CV_SIFT> _sift;
 };
 
 //ORB
@@ -190,8 +229,8 @@ private:
 	int fastThreshold_;
 	bool nonmaxSuppresion_;
 
-	cv::ORB * _orb;
-	cv::gpu::ORB_GPU * _gpuOrb;
+	cv::Ptr<CV_ORB> _orb;
+	cv::Ptr<CV_ORB_GPU> _gpuOrb;
 };
 
 //FAST
@@ -212,8 +251,8 @@ private:
 	bool gpu_;
 	double gpuKeypointsRatio_;
 
-	cv::FastFeatureDetector * _fast;
-	cv::gpu::FAST_GPU * _gpuFast;
+	cv::Ptr<CV_FAST> _fast;
+	cv::Ptr<CV_FAST_GPU> _gpuFast;
 };
 
 //FAST_BRIEF
@@ -232,7 +271,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //FAST_FREAK
@@ -254,7 +293,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //GFTT
@@ -277,7 +316,7 @@ private:
 	bool _useHarrisDetector;
 	double _k;
 
-	cv::GFTTDetector * _gftt;
+	cv::Ptr<CV_GFTT> _gftt;
 };
 
 //GFTT_BRIEF
@@ -296,7 +335,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //GFTT_FREAK
@@ -318,7 +357,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //BRISK
@@ -340,10 +379,10 @@ private:
 	int octaves_;
 	float patternScale_;
 
-	cv::BRISK * brisk_;
+	cv::Ptr<CV_BRISK> brisk_;
 };
 
 
 }
 
-#endif /* KEYPOINTDESCRIPTOR_H_ */
+#endif /* FEATURES2D_H_ */

corelib/include/rtabmap/core/OdometryInfo.h

@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ODOMETRYINFO_H_
 #define ODOMETRYINFO_H_
 
+#include <map>
+
 namespace rtabmap {
 
 class OdometryInfo

corelib/include/rtabmap/core/util3d.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
 #include <opencv2/core/core.hpp>
+#include <map>
 #include <list>
 
 namespace rtabmap

corelib/include/rtabmap/core/util3d_features.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
 #include <list>
+#include <map>
 
 namespace rtabmap
 {
@@ -95,7 +96,7 @@ std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
 		Transform & cameraTransform,
 		int pnpIterations = 100,
 		float pnpReprojError = 8.0f,
-		int pnpFlags = cv::ITERATIVE,
+		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
 		float ransacParam1 = 3.0f,
 		float ransacParam2 = 0.99f,
 		const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
@@ -105,14 +106,6 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
-		const cv::Mat & image,
-		const cv::Mat & imageDepth,
-		float constant,
-		int fastThreshold=50,
-		bool fastNonmaxSuppression=true,
-		float maxDepth = 5.0f);
-
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_mapping.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h"
 
 #include <opencv2/core/core.hpp>
+#include <map>
 #include <rtabmap/core/Transform.h>
 #include <pcl/pcl_base.h>
 #include <pcl/point_cloud.h>

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

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

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(),

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
 			{

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
 {

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

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