/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include <opencv2/core/version.hpp>
#include <opencv2/opencv_modules.hpp>

#ifdef RTABMAP_ORB_OCTREE
#include "opencv/ORBextractor.h"
#endif

#ifdef RTABMAP_TORCH
#include "superpoint_torch/SuperPoint.h"
#endif
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
#include "superpoint_rpautrat/SuperpointRpautrat.h"
#endif

#ifdef RTABMAP_PYTHON
#include "python/PyDetector.h"
#endif

#if CV_MAJOR_VERSION < 3
#include "opencv/Orb.h"
#ifdef HAVE_OPENCV_GPU
#include <opencv2/gpu/gpu.hpp>
#endif
#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
#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(
		std::vector<cv::KeyPoint> & keypoints,
		const cv::Mat & depth,
		float minDepth,
		float maxDepth)
{
	cv::Mat descriptors;
	filterKeypointsByDepth(keypoints, descriptors, depth, minDepth, maxDepth);
}

void Feature2D::filterKeypointsByDepth(
		std::vector<cv::KeyPoint> & keypoints,
		cv::Mat & descriptors,
		const cv::Mat & depth,
		float minDepth,
		float maxDepth)
{
	UASSERT(minDepth >= 0.0f);
	UASSERT(maxDepth <= 0.0f || maxDepth > minDepth);
	if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
	{
		std::vector<cv::KeyPoint> output(keypoints.size());
		std::vector<int> indexes(keypoints.size(), 0);
		int oi=0;
		bool isInMM = depth.type() == CV_16UC1;
		for(unsigned int i=0; i<keypoints.size(); ++i)
		{
			int u = int(keypoints[i].pt.x+0.5f);
			int v = int(keypoints[i].pt.y+0.5f);
			if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
			{
				float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
				if(uIsFinite(d) && d>minDepth && (maxDepth <= 0.0f || d < maxDepth))
				{
					output[oi++] = keypoints[i];
					indexes[i] = 1;
				}
			}
		}
		output.resize(oi);
		keypoints = output;

		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
		{
			if(keypoints.size() == 0)
			{
				descriptors = cv::Mat();
			}
			else
			{
				cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
				int di = 0;
				for(unsigned int i=0; i<indexes.size(); ++i)
				{
					if(indexes[i] == 1)
					{
						if(descriptors.type() == CV_32FC1)
						{
							memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
						}
						else // CV_8UC1
						{
							memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
						}
					}
				}
				descriptors = newDescriptors;
			}
		}
	}
}

void Feature2D::filterKeypointsByDepth(
		std::vector<cv::KeyPoint> & keypoints,
		cv::Mat & descriptors,
		std::vector<cv::Point3f> & keypoints3D,
		float minDepth,
		float maxDepth)
{
	UDEBUG("");
	//remove all keypoints/descriptors with no valid 3D points
	UASSERT(((int)keypoints.size() == descriptors.rows || descriptors.empty()) &&
			keypoints3D.size() == keypoints.size());
	std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
	std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
	cv::Mat validDescriptors(descriptors.size(), descriptors.type());

	int oi=0;
	float minDepthSqr = minDepth * minDepth;
	float maxDepthSqr = maxDepth * maxDepth;
	for(unsigned int i=0; i<keypoints3D.size(); ++i)
	{
		cv::Point3f & pt = keypoints3D[i];
		if(util3d::isFinite(pt))
		{
			float distSqr = pt.x*pt.x+pt.y*pt.y+pt.z*pt.z;
			if(distSqr >= minDepthSqr && (maxDepthSqr==0.0f || distSqr <= maxDepthSqr))
			{
				validKeypoints[oi] = keypoints[i];
				validKeypoints3D[oi] = pt;
				if(!descriptors.empty())
				{
					descriptors.row(i).copyTo(validDescriptors.row(oi));
				}
				++oi;
			}
		}
	}
	UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
	validKeypoints.resize(oi);
	validKeypoints3D.resize(oi);
	keypoints = validKeypoints;
	keypoints3D = validKeypoints3D;
	if(!descriptors.empty())
	{
		descriptors = validDescriptors.rowRange(0, oi).clone();
	}
}

void Feature2D::filterKeypointsByDisparity(
		std::vector<cv::KeyPoint> & keypoints,
		const cv::Mat & disparity,
		float minDisparity)
{
	cv::Mat descriptors;
	filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
}

void Feature2D::filterKeypointsByDisparity(
		std::vector<cv::KeyPoint> & keypoints,
		cv::Mat & descriptors,
		const cv::Mat & disparity,
		float minDisparity)
{
	if(!disparity.empty() && minDisparity > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
	{
		std::vector<cv::KeyPoint> output(keypoints.size());
		std::vector<int> indexes(keypoints.size(), 0);
		int oi=0;
		for(unsigned int i=0; i<keypoints.size(); ++i)
		{
			int u = int(keypoints[i].pt.x+0.5f);
			int v = int(keypoints[i].pt.y+0.5f);
			if(u >=0 && u<disparity.cols && v >=0 && v<disparity.rows)
			{
				float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
				if(d!=0.0f && uIsFinite(d) && d >= minDisparity)
				{
					output[oi++] = keypoints[i];
					indexes[i] = 1;
				}
			}
		}
		output.resize(oi);
		keypoints = output;

		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
		{
			if(keypoints.size() == 0)
			{
				descriptors = cv::Mat();
			}
			else
			{
				cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
				int di = 0;
				for(unsigned int i=0; i<indexes.size(); ++i)
				{
					if(indexes[i] == 1)
					{
						if(descriptors.type() == CV_32FC1)
						{
							memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
						}
						else // CV_8UC1
						{
							memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
						}
					}
				}
				descriptors = newDescriptors;
			}
		}
	}
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
	cv::Mat descriptors;
	limitKeypoints(keypoints, descriptors, maxKeypoints, imageSize, ssc);
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
	std::vector<cv::Point3f> keypoints3D;
	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints, imageSize, ssc);
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
	UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
	{
		UTimer timer;
		int removed;
		std::vector<cv::KeyPoint> kptsTmp;
		std::vector<cv::Point3f> kpts3DTmp;
		cv::Mat descriptorsTmp;
		if(ssc)
		{
			ULOGGER_DEBUG("too many words (%d), removing words with SSC", (int)keypoints.size());

			// Sorting keypoints by deacreasing order of strength
			std::vector<float> responseVector;
			for (unsigned int i = 0; i < keypoints.size(); i++)
			{
				responseVector.push_back(keypoints[i].response);
			}
			std::vector<int> indx(responseVector.size());
			std::iota(std::begin(indx), std::end(indx), 0);

#if CV_MAJOR_VERSION >= 4
			cv::sortIdx(responseVector, indx, cv::SORT_DESCENDING);
#else
			cv::sortIdx(responseVector, indx, CV_SORT_DESCENDING);
#endif

			static constexpr float tolerance = 0.1;
			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height, indx);
			removed = keypoints.size()-ResultVec.size();
			// retrieve final keypoints
			kptsTmp.resize(ResultVec.size());
			if(!keypoints3D.empty())
			{
				kpts3DTmp.resize(ResultVec.size());
			}
			if(descriptors.rows)
			{
				descriptorsTmp = cv::Mat(ResultVec.size(), descriptors.cols, descriptors.type());
			}
			for(unsigned int k=0; k<ResultVec.size(); ++k)
			{
				kptsTmp[k] = keypoints[ResultVec[k]];
				if(keypoints3D.size())
				{
					kpts3DTmp[k] = keypoints3D[ResultVec[k]];
				}
				if(descriptors.rows)
				{
					if(descriptors.type() == CV_32FC1)
					{
						memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(ResultVec[k]), descriptors.cols*sizeof(float));
					}
					else
					{
						memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(ResultVec[k]), descriptors.cols*sizeof(char));
					}
				}
			}
		}
		else
		{
			ULOGGER_DEBUG("too many words (%d), removing words with the hessian threshold", (int)keypoints.size());
			// Remove words under the new hessian threshold

			// Sort words by hessian
			std::multimap<float, int> hessianMap; // <hessian,id>
			for(unsigned int i = 0; i <keypoints.size(); ++i)
			{
				//Keep track of the data, to be easier to manage the data in the next step
				hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
			}

			// Remove them from the signature
			removed = (int)hessianMap.size()-maxKeypoints;
			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
			kptsTmp.resize(maxKeypoints);
			if(!keypoints3D.empty())
			{
				kpts3DTmp.resize(maxKeypoints);
			}
			if(descriptors.rows)
			{
				descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
			}
			for(unsigned int k=0; k<kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
			{
				kptsTmp[k] = keypoints[iter->second];
				if(keypoints3D.size())
				{
					kpts3DTmp[k] = keypoints3D[iter->second];
				}
				if(descriptors.rows)
				{
					if(descriptors.type() == CV_32FC1)
					{
						memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
					}
					else
					{
						memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
					}
				}
			}
		}
		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), !ssc&&kptsTmp.size()?kptsTmp.back().response:0.0f);
		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
		keypoints = kptsTmp;
		keypoints3D = kpts3DTmp;
		if(descriptors.rows)
		{
			descriptors = descriptorsTmp;
		}
	}
}

void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
	{
		UTimer timer;
		float minimumHessian = 0.0f;
		int removed;
		inliers.resize(keypoints.size(), false);
		if(ssc)
		{
			ULOGGER_DEBUG("too many words (%d), removing words with SSC", (int)keypoints.size());

			// Sorting keypoints by deacreasing order of strength
			std::vector<float> responseVector;
			for (unsigned int i = 0; i < keypoints.size(); i++)
			{
				responseVector.push_back(keypoints[i].response);
			}
			std::vector<int> indx(responseVector.size());
			std::iota(std::begin(indx), std::end(indx), 0);

#if CV_MAJOR_VERSION >= 4
			cv::sortIdx(responseVector, indx, cv::SORT_DESCENDING);
#else
			cv::sortIdx(responseVector, indx, CV_SORT_DESCENDING);
#endif

			static constexpr float tolerance = 0.1;
			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height, indx);
			removed = keypoints.size()-ResultVec.size();
			for(unsigned int k=0; k<ResultVec.size(); ++k)
			{
				inliers[ResultVec[k]] = true;
			}
		}
		else
		{
			ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
			// Remove words under the new hessian threshold

			// Sort words by hessian
			std::multimap<float, int> hessianMap; // <hessian,id>
			for(unsigned int i = 0; i<keypoints.size(); ++i)
			{
				//Keep track of the data, to be easier to manage the data in the next step
				hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
			}

			// Keep keypoints with highest response
			removed = (int)hessianMap.size()-maxKeypoints;
			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
			for(int k=0; k<maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
			{
				inliers[iter->second] = true;
				minimumHessian = iter->first;
			}
		}
		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)(keypoints.size()-removed), minimumHessian);
		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
	}
	else
	{
		ULOGGER_DEBUG("keeping all %d keypoints", (int)keypoints.size());
		inliers.resize(keypoints.size(), true);
	}
}

void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc)
{
	if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
	{
		inliers.resize(keypoints.size(), true);
		return;
	}
	UASSERT(gridCols>=1 && gridRows >=1);
	UASSERT(imageSize.height>gridRows && imageSize.width>gridCols);
	int rowSize = imageSize.height / gridRows;
	int colSize = imageSize.width / gridCols;
	int maxKeypointsPerCell = maxKeypoints / (gridRows * gridCols);
	std::vector<std::vector<cv::KeyPoint> > keypointsPerCell(gridRows * gridCols);
	std::vector<std::vector<int> > indexesPerCell(gridRows * gridCols);
	for(size_t i=0; i<keypoints.size(); ++i)
	{
		int cellRow = int(keypoints[i].pt.y)/rowSize;
		int cellCol = int(keypoints[i].pt.x)/colSize;
		UASSERT(cellRow >=0 && cellRow < gridRows);
		UASSERT(cellCol >=0 && cellCol < gridCols);

		keypointsPerCell[cellRow*gridCols + cellCol].push_back(keypoints[i]);
		indexesPerCell[cellRow*gridCols + cellCol].push_back(i);
	}
	inliers.resize(keypoints.size(), false);
	for(size_t i=0; i<keypointsPerCell.size(); ++i)
	{
		std::vector<bool> inliersCell;
		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell, cv::Size(colSize, rowSize), ssc);
		for(size_t j=0; j<inliersCell.size(); ++j)
		{
			if(inliersCell[j])
			{
				inliers.at(indexesPerCell[i][j]) = true;
			}
		}
	}
}

cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
{
	return util2d::computeRoi(image, roiRatios);
}

cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
	return util2d::computeRoi(image, roiRatios);
}

/////////////////////
// Feature2D
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
		maxFeatures_(Parameters::defaultKpMaxFeatures()),
		SSC_(Parameters::defaultKpSSC()),
		_maxDepth(Parameters::defaultKpMaxDepth()),
		_minDepth(Parameters::defaultKpMinDepth()),
		_roiRatios(std::vector<float>(4, 0.0f)),
		_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
		_subPixIterations(Parameters::defaultKpSubPixIterations()),
		_subPixEps(Parameters::defaultKpSubPixEps()),
		gridRows_(Parameters::defaultKpGridRows()),
		gridCols_(Parameters::defaultKpGridCols())
{
	_stereo = new Stereo(parameters);
	this->parseParameters(parameters);
}
Feature2D::~Feature2D()
{
	delete _stereo;
}
void Feature2D::parseParameters(const ParametersMap & parameters)
{
	uInsert(parameters_, parameters);

	Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
	Parameters::parse(parameters, Parameters::kKpSSC(), SSC_);
	Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
	Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
	Parameters::parse(parameters, Parameters::kKpGridRows(), gridRows_);
	Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);

	UASSERT(gridRows_ >= 1 && gridCols_>=1);

	// convert ROI from string to vector
	ParametersMap::const_iterator iter;
	if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
	{
		std::list<std::string> strValues = uSplit(iter->second, ' ');
		if(strValues.size() != 4)
		{
			ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", iter->second.c_str());
		}
		else
		{
			std::vector<float> tmpValues(4);
			unsigned int i=0;
			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
			{
				tmpValues[i] = uStr2Float(*jter);
				++i;
			}

			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
			{
				_roiRatios = tmpValues;
			}
			else
			{
				ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", iter->second.c_str());
			}
		}
	}

	//stereo
	UASSERT(_stereo != 0);
	if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
	{
		delete _stereo;
		_stereo = Stereo::create(parameters_);
	}
	else
	{
		_stereo->parseParameters(parameters);
	}
}
Feature2D * Feature2D::create(const ParametersMap & parameters)
{
	int type = Parameters::defaultKpDetectorStrategy();
	Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
	return create((Feature2D::Type)type, parameters);
}

bool Feature2D::isAvailable(Feature2D::Type type)
{
	// kFeatureUndef is a sentinel ("strategy not specified"); create() falls
	// through to a default backend, so the type isn't really "available" as
	// requested.
	if(type == kFeatureUndef)
	{
		return false;
	}

	// SURF / SIFT / SURF-FREAK / SURF-DAISY require either OpenCV < 3.4.11
	// (built-in) OR the xfeatures2d module + RTABMAP_NONFREE for OpenCV >= 3.4.11.
#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)))
  #ifndef RTABMAP_NONFREE
	if(type == kFeatureSurf || type == kFeatureSift || type == kFeatureSurfFreak || type == kFeatureSurfDaisy)
	{
		return false;
	}
  #endif
#else
  #ifndef RTABMAP_NONFREE
	if(type == kFeatureSurf || type == kFeatureSurfFreak || type == kFeatureSurfDaisy)
	{
		return false;
	}
  #endif
#endif

#if !defined(HAVE_OPENCV_XFEATURES2D) && CV_MAJOR_VERSION >= 3
	if(type == kFeatureFastBrief ||
	   type == kFeatureFastFreak ||
	   type == kFeatureGfttBrief ||
	   type == kFeatureGfttFreak ||
	   type == kFeatureSurfFreak ||
	   type == kFeatureGfttDaisy ||
	   type == kFeatureSurfDaisy)
	{
		return false;
	}
#elif CV_MAJOR_VERSION < 3
	if(type == kFeatureKaze ||
	   type == kFeatureGfttDaisy ||
	   type == kFeatureSurfDaisy)
	{
		return false;
	}
#endif

#ifndef RTABMAP_ORB_OCTREE
	if(type == kFeatureOrbOctree) return false;
#endif
#ifndef RTABMAP_TORCH
	if(type == kFeatureSuperPointTorch) return false;
#endif
#if !defined(RTABMAP_TORCH) || !defined(RTABMAP_PYTHON)
	if(type == kFeatureSuperPointRpautrat) return false;
#endif
#ifndef RTABMAP_PYTHON
	if(type == kFeaturePyDetector) return false;
#endif
	return true;
}
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{

// NONFREE checks
#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)))

  #ifndef RTABMAP_NONFREE
	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift || type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
	{
    #if CV_MAJOR_VERSION < 3
		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
    #else
		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
    #endif
		type = Feature2D::kFeatureGfttOrb;
	}
  #endif

#else // >= 4.4.0 >= 3.4.11

  #ifndef RTABMAP_NONFREE
	if(type == Feature2D::kFeatureSurf)
	{
		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
		type = Feature2D::kFeatureSift;
	}
	else if(type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
	{
		UWARN("SURF detector cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
		type = Feature2D::kFeatureGfttOrb;
	}
  #endif

#endif // >= 4.4.0 >= 3.4.11

#if !defined(HAVE_OPENCV_XFEATURES2D) && CV_MAJOR_VERSION >= 3
	if(type == Feature2D::kFeatureFastBrief ||
	   type == Feature2D::kFeatureFastFreak ||
	   type == Feature2D::kFeatureGfttBrief ||
	   type == Feature2D::kFeatureGfttFreak ||
	   type == Feature2D::kFeatureSurfFreak ||
	   type == Feature2D::kFeatureGfttDaisy ||
	   type == Feature2D::kFeatureSurfDaisy)
	{
		UWARN("BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
		type = Feature2D::kFeatureGfttOrb;
	}
#elif CV_MAJOR_VERSION < 3
	if(type == Feature2D::kFeatureKaze)
	{
  #ifdef RTABMAP_NONFREE
		UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
		type = Feature2D::kFeatureSurf;
  #else
		UWARN("KAZE detector/descriptor can be used only with OpenCV3. GFTT/ORB is used instead.");
		type = Feature2D::kFeatureGfttOrb;
  #endif
	}
	if(type == Feature2D::kFeatureGfttDaisy || type == Feature2D::kFeatureSurfDaisy)
	{
		UWARN("DAISY detector/descriptor can be used only with OpenCV3. GFTT/BRIEF is used instead.");
		type = Feature2D::kFeatureGfttBrief;
	}
#endif


#ifndef RTABMAP_ORB_OCTREE
	if(type == Feature2D::kFeatureOrbOctree)
	{
		UWARN("ORB OcTree feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
		type = Feature2D::kFeatureGfttOrb;
	}
#endif

#ifndef RTABMAP_TORCH
	if(type == Feature2D::kFeatureSuperPointTorch)
	{
		UWARN("SuperPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
		type = Feature2D::kFeatureGfttOrb;
	}
#endif

	Feature2D * feature2D = 0;
	switch(type)
	{
	case Feature2D::kFeatureSurf:
		feature2D = new SURF(parameters);
		break;
	case Feature2D::kFeatureSift:
		feature2D = new SIFT(parameters);
		break;
	case Feature2D::kFeatureOrb:
		feature2D = new ORB(parameters);
		break;
	case Feature2D::kFeatureFastBrief:
		feature2D = new FAST_BRIEF(parameters);
		break;
	case Feature2D::kFeatureFastFreak:
		feature2D = new FAST_FREAK(parameters);
		break;
	case Feature2D::kFeatureGfttFreak:
		feature2D = new GFTT_FREAK(parameters);
		break;
	case Feature2D::kFeatureGfttBrief:
		feature2D = new GFTT_BRIEF(parameters);
		break;
	case Feature2D::kFeatureGfttOrb:
		feature2D = new GFTT_ORB(parameters);
		break;
	case Feature2D::kFeatureBrisk:
		feature2D = new BRISK(parameters);
		break;
	case Feature2D::kFeatureKaze:
		feature2D = new KAZE(parameters);
		break;
	case Feature2D::kFeatureOrbOctree:
		feature2D = new ORBOctree(parameters);
		break;
#ifdef RTABMAP_TORCH
case Feature2D::kFeatureSuperPointTorch:
    feature2D = new SuperPointTorch(parameters);
    break;
#endif
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
case Feature2D::kFeatureSuperPointRpautrat:
    feature2D = new SuperPointRpautrat(parameters);
    break;
#endif
	case Feature2D::kFeatureSurfFreak:
		feature2D = new SURF_FREAK(parameters);
		break;
	case Feature2D::kFeatureGfttDaisy:
		feature2D = new GFTT_DAISY(parameters);
		break;
	case Feature2D::kFeatureSurfDaisy:
		feature2D = new SURF_DAISY(parameters);
		break;
#ifdef RTABMAP_PYTHON
	case Feature2D::kFeaturePyDetector:
		feature2D = new PyDetector(parameters);
		break;
#endif
#ifdef RTABMAP_NONFREE
	default:
		feature2D = new SURF(parameters);
		type = Feature2D::kFeatureSurf;
		break;
#else
	default:
		feature2D = new ORB(parameters);
		type = Feature2D::kFeatureGfttOrb;
		break;
#endif

	}
	return feature2D;
}

std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Mat & maskIn)
{
	UASSERT(!image.empty());
	UASSERT(image.type() == CV_8UC1);

	cv::Mat mask;
	if(!maskIn.empty())
	{
		if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
		{
			mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
			for(int i=0; i<(int)mask.total(); ++i)
			{
				float value = 0.0f;
				if(maskIn.type()==CV_16UC1)
				{
					if(((unsigned short*)maskIn.data)[i] > 0 &&
					   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
					{
						value = float(((unsigned short*)maskIn.data)[i])*0.001f;
					}
				}
				else
				{
					value = ((float*)maskIn.data)[i];
				}

				if(value>_minDepth &&
				   (_maxDepth == 0.0f || value <= _maxDepth) &&
				   uIsFinite(value))
				{
					((unsigned char*)mask.data)[i] = 255; // ORB uses 255 to handle pyramids
				}
			}
		}
		else if(maskIn.type()==CV_8UC1)
		{
			// assume a standard mask
			mask = maskIn;
		}
		else
		{
			UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
		}
	}

	UASSERT(mask.empty() || (mask.cols == image.cols && mask.rows == image.rows));

	std::vector<cv::KeyPoint> keypoints;
	UTimer timer;
	cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios);
	if(!(globalRoi.width && globalRoi.height))
	{
		globalRoi = cv::Rect(0,0,image.cols, image.rows);
	}

	// Get keypoints
	int rowSize = globalRoi.height / gridRows_;
	int colSize = globalRoi.width / gridCols_;
	int maxFeatures =	maxFeatures_ / (gridRows_ * gridCols_);
	for (int i = 0; i<gridRows_; ++i)
	{
		for (int j = 0; j<gridCols_; ++j)
		{
			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
			std::vector<cv::KeyPoint> subKeypoints;
			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
			if (this->getType() != Feature2D::Type::kFeaturePyDetector && this->getType() != Feature2D::Type::kFeatureSuperPointRpautrat)
			{
				limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
			}
			if(roi.x || roi.y)
			{
				// Adjust keypoint position to raw image
				for(std::vector<cv::KeyPoint>::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter)
				{
					iter->pt.x += roi.x;
					iter->pt.y += roi.y;
				}
			}
			keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() );
		}
	}
	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
			timer.ticks(), (int)keypoints.size(), gridCols_, gridRows_,  mask.empty()?1:0);

	if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
	{
		std::vector<cv::Point2f> corners;
		cv::KeyPoint::convert(keypoints, corners);
		cv::cornerSubPix( image, corners,
				cv::Size( _subPixWinSize, _subPixWinSize ),
				cv::Size( -1, -1 ),
				cv::TermCriteria( cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, _subPixIterations, _subPixEps ) );

		for(unsigned int i=0;i<corners.size(); ++i)
		{
			keypoints[i].pt = corners[i];
		}
		UDEBUG("subpixel time = %f s", timer.ticks());
	}

	return keypoints;
}

cv::Mat Feature2D::generateDescriptors(
		const cv::Mat & image,
		std::vector<cv::KeyPoint> & keypoints) const
{
	cv::Mat descriptors;
	if(keypoints.size())
	{
		UASSERT(!image.empty());
		UASSERT(image.type() == CV_8UC1);
		descriptors = generateDescriptorsImpl(image, keypoints);
		if(descriptors.rows != (int)keypoints.size())
		{
			UWARN("Descriptor extraction returned %d rows for %d keypoints — "
					"clearing keypoints to keep them in sync.",
					descriptors.rows, (int)keypoints.size());
			keypoints.clear();
			descriptors = cv::Mat();
		}
		else {
			UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
		}
	}
	return descriptors;
}

std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
		const SensorData & data,
		const std::vector<cv::KeyPoint> & keypoints) const
{
	std::vector<cv::Point3f> keypoints3D;
	if(keypoints.size())
	{
		if(!data.rightRaw().empty() && !data.imageRaw().empty() &&
			!data.stereoCameraModels().empty() &&
			data.stereoCameraModels()[0].isValidForProjection())
		{
			//stereo
			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())
			{
				d_imageLeft = data.imageRawGpu();
				if(d_imageLeft.empty()) {
					d_imageLeft = cv::cuda::GpuMat(imageLeft);
				}
				// convert to grayscale if not already
				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);
				}
				// convert to grayscale if not already
				if(d_imageRight.channels() > 1) {
					cv::cuda::GpuMat tmp;
					cv::cuda::cvtColor(d_imageRight, tmp, cv::COLOR_BGR2GRAY);
					d_imageRight = tmp;
				}
			}
			else
#endif
			{
				// convert to grayscale
				if(imageLeft.channels() > 1)
				{
					cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
				}
				if(imageRight.channels() > 1)
				{
					cv::cvtColor(data.rightRaw(), imageRight, cv::COLOR_BGR2GRAY);
				}
			}

			std::vector<cv::Point2f> leftCorners;
			cv::KeyPoint::convert(keypoints, leftCorners);

			std::vector<cv::Point2f> rightCorners;

			if(data.stereoCameraModels().size() == 1)
			{
				std::vector<unsigned char> 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)
				{
					int rejected = 0;
					for(size_t i=0; i<status.size(); ++i)
					{
						if(status[i]==0)
						{
							++rejected;
						}
					}
					if(rejected > (int)status.size()/2)
					{
						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
								"Optical flow may have failed because images are not calibrated, "
								"the background is too far (no disparity between the images), "
								"maximum disparity may be too small (%f) or that exposure between "
								"left and right images is too different.",
								rejected,
								(int)status.size(),
								_stereo->maxDisparity());
					}
				}

				keypoints3D = util3d::generateKeypoints3DStereo(
						leftCorners,
						rightCorners,
						data.stereoCameraModels()[0],
						status,
						_minDepth,
						_maxDepth);
			}
			else
			{
				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
				for(size_t i=0; i<leftCorners.size(); ++i)
				{
					int cameraIndex = int(leftCorners[i].x / subImageWith);
					leftCorners[i].x -= cameraIndex*subImageWith;
					subLeftCorners[cameraIndex].push_back(leftCorners[i]);
					subIndex[cameraIndex].push_back(i);
				}

				keypoints3D.resize(keypoints.size());
				int total = 0;
				int rejected = 0;
				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
				{
					if(!subLeftCorners[i].empty())
					{
						std::vector<unsigned char> status;
#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],
								rightCorners,
								data.stereoCameraModels()[i],
								status,
								_minDepth,
								_maxDepth);

						if(ULogger::level() >= ULogger::kWarning)
						{
							for(size_t i=0; i<status.size(); ++i)
							{
								if(status[i]==0)
								{
									++rejected;
								}
							}
							total+=status.size();
						}

						UASSERT(subIndex[i].size() == subKeypoints3D.size());
						for(size_t j=0; j<subKeypoints3D.size(); ++j)
						{
							keypoints3D[subIndex[i][j]] = subKeypoints3D[j];
						}
					}
				}

				if(ULogger::level() >= ULogger::kWarning)
				{
					if(rejected > total/2)
					{
						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
								"Optical flow may have failed because images are not calibrated, "
								"the background is too far (no disparity between the images), "
								"maximum disparity may be too small (%f) or that exposure between "
								"left and right images is too different.",
								rejected,
								total,
								_stereo->maxDisparity());
					}
				}
			}
		}
		else if(!data.depthRaw().empty() && data.cameraModels().size())
		{
			keypoints3D = util3d::generateKeypoints3DDepth(
					keypoints,
					data.depthOrRightRaw(),
					data.cameraModels(),
					_minDepth,
					_maxDepth);
		}
	}

	return keypoints3D;
}

//////////////////////////
//SURF
//////////////////////////
SURF::SURF(const ParametersMap & parameters) :
		hessianThreshold_(Parameters::defaultSURFHessianThreshold()),
		nOctaves_(Parameters::defaultSURFOctaves()),
		nOctaveLayers_(Parameters::defaultSURFOctaveLayers()),
		extended_(Parameters::defaultSURFExtended()),
		upright_(Parameters::defaultSURFUpright()),
		gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
		gpuVersion_(Parameters::defaultSURFGpuVersion())
{
	parseParameters(parameters);
}

SURF::~SURF()
{
}

void SURF::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
	Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves_);
	Parameters::parse(parameters, Parameters::kSURFUpright(), upright_);
	Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);

#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of SURF not available! Using CPU version instead...");
		gpuVersion_ = false;
	}
#else
	if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of SURF not available! Using CPU version instead...");
		gpuVersion_ = false;
	}
#endif
	if(gpuVersion_)
	{
		_gpuSurf = cv::Ptr<CV_SURF_GPU>(new CV_SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_));
	}
	else
	{
#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!");
#endif
}

bool SURF::isGpuAvailable() const
{
#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
	return cv::gpu::getCudaEnabledDeviceCount() > 0;
#else
	return cv::cuda::getCudaEnabledDeviceCount() > 0;
#endif
#else
	return false;
#endif
}

std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	std::vector<cv::KeyPoint> keypoints;

#ifdef RTABMAP_NONFREE
	cv::Mat imgRoi(image, roi);
	cv::Mat maskRoi;
	if(!mask.empty())
	{
		maskRoi = cv::Mat(mask, roi);
	}
	if(gpuVersion_)
	{
#if CV_MAJOR_VERSION < 3
		cv::gpu::GpuMat imgGpu(imgRoi);
		cv::gpu::GpuMat maskGpu(maskRoi);
		(*_gpuSurf.obj)(imgGpu, maskGpu, keypoints);
#else
		cv::cuda::GpuMat imgGpu(imgRoi);
		cv::cuda::GpuMat maskGpu(maskRoi);
		(*_gpuSurf.get())(imgGpu, maskGpu, keypoints);
#endif
	}
	else
	{
		_surf->detect(imgRoi, keypoints, maskRoi);
	}
#else
	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif
	return keypoints;
}

cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	cv::Mat descriptors;
#ifdef RTABMAP_NONFREE
	if(gpuVersion_)
	{
#if CV_MAJOR_VERSION < 3
		cv::gpu::GpuMat imgGpu(image);
		cv::gpu::GpuMat descriptorsGPU;
		(*_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())
			descriptors = cv::Mat();
		else
		{
			UASSERT(descriptorsGPU.type() == CV_32F);
			descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
			descriptorsGPU.download(descriptors);
		}
	}
	else
	{
		_surf->compute(image, keypoints, descriptors);
	}
#else
	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif

	return descriptors;
}

//////////////////////////
//SIFT
//////////////////////////
SIFT::SIFT(const ParametersMap & parameters) :
	nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
	sigma_(Parameters::defaultSIFTSigma()),
	preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
	rootSIFT_(Parameters::defaultSIFTRootSIFT()),
	gpu_(Parameters::defaultSIFTGpu()),
	gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
	maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
	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)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
	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_);
	Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
	Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
	Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
	
	if(gpu_)
	{
#ifdef RTABMAP_CUDASIFT
		// Check if there is a cuda device
		if(cudaSiftData_==0)
		{
			if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
				UDEBUG("Init SiftData");
				cudaSiftData_ = new SiftData();
				InitSiftData(*cudaSiftData_, 8192, true, true);
			}
			else{
				UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
				gpu_ = false;
			}
		}
#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_));
#else
		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!");
#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_);
#endif
	}

}

bool SIFT::isGpuAvailable() const
{
#ifdef RTABMAP_CUDASIFT
	return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
	return false;
#endif
}

std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	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())
	{
		maskRoi = cv::Mat(mask, roi);
	}
#ifdef RTABMAP_CUDASIFT
	if(gpu_)
	{
		/* 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 = gaussianThreshold_;   /* 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)
		{
			keypoints.resize(cudaSiftData_->numPts);
			cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
			size_t k=0;
			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;
				}
				// Ignore keypoints not in the mask
				if(!maskRoi.empty() && maskRoi.at<unsigned char>(cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].xpos) == 0)
				{
					continue;
				}

				if(i>0 && 
					cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
				{
					// Same feature, skip doubles
					continue;
				}

				float response = abs(cudaSiftData_->h_data[i].sharpness);
				if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
				{
					continue;
				}

				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 = response; 
				keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
				++k;
			}
			if(k < keypoints.size())
			{
				UDEBUG("keypoints extracted = %d, valid=%d", (int)keypoints.size(), (int)k);
				keypoints.resize(k);
				cudaSiftDescriptors_.resize(k);
			}
			if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
			{
				// Call limitKeypoints() now to filter the descriptors.
				this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
			}
		}
	}
	else
#endif
	{
#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
#else
		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
#endif
	}
	return keypoints;
}

cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
	cv::Mat descriptors;
#ifdef RTABMAP_CUDASIFT
	if(gpu_)
	{
		if((int)keypoints.size() == cudaSiftDescriptors_.rows)
		{
			descriptors = cudaSiftDescriptors_.clone();
		}
		else
		{
			UERROR("CudaSift: keypoints size %ld is not equal to extracted descriptors size %d", keypoints.size(), cudaSiftDescriptors_.rows);
			return cv::Mat();
		}
	}
	else
	{
#endif

		UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
#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);
#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);
#endif

#ifdef RTABMAP_CUDASIFT
	}
#endif

	if( rootSIFT_ && !descriptors.empty())
	{
		UDEBUG("Performing RootSIFT...");
		// see http://www.pyimagesearch.com/2015/04/13/implementing-rootsift-in-python-and-opencv/
		// apply the Hellinger kernel by first L1-normalizing and taking the
		// square-root
		for(int i=0; i<descriptors.rows; ++i)
		{
			// By taking the L1 norm, followed by the square-root, we have
			// already L2 normalized the feature vector and further normalization
			// is not needed.
			descriptors.row(i) = descriptors.row(i) / cv::sum(descriptors.row(i))[0];
			cv::sqrt(descriptors.row(i), descriptors.row(i));
		}
	}
	return descriptors;
}

//////////////////////////
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
		scaleFactor_(Parameters::defaultORBScaleFactor()),
		nLevels_(Parameters::defaultORBNLevels()),
		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
		firstLevel_(Parameters::defaultORBFirstLevel()),
		WTA_K_(Parameters::defaultORBWTA_K()),
		scoreType_(Parameters::defaultORBScoreType()),
		patchSize_(Parameters::defaultORBPatchSize()),
		gpu_(Parameters::defaultORBGpu()),
		fastThreshold_(Parameters::defaultFASTThreshold()),
		nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression())
{
	parseParameters(parameters);
}

ORB::~ORB()
{
}

void ORB::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
	Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel_);
	Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K_);
	Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType_);
	Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
	Parameters::parse(parameters, Parameters::kORBGpu(), gpu_);

	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);

#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of ORB not available! Using CPU version instead...");
		gpu_ = false;
	}
#else
	if(gpu_)
	{
		UWARN("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
		gpu_ = false;
	}
#endif
#else
#ifndef HAVE_OPENCV_CUDAFEATURES2D
	if(gpu_)
	{
		UWARN("GPU version of ORB not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
		gpu_ = false;
	}
#endif
	if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of ORB not available (no GPU found)! Using CPU version instead...");
		gpu_ = false;
	}
#endif
	if(gpu_)
	{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
		_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
		_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
#else
		UFATAL("not supposed to be here");
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
		_gpuOrb = CV_ORB_GPU::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
#endif
#endif
	}
	else
	{
#if CV_MAJOR_VERSION < 3
		_orb = cv::Ptr<CV_ORB>(new CV_ORB(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, parameters));
#elif CV_MAJOR_VERSION > 3
		_orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, (cv::ORB::ScoreType)scoreType_, patchSize_, fastThreshold_);
#else
		_orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
#endif
	}
}

bool ORB::isGpuAvailable() const
{
#ifdef HAVE_OPENCV_CUDAFEATURES2D
	return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
	return false;
#endif
}

std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	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())
	{
		maskRoi = cv::Mat(mask, roi);
	}

	if(gpu_)
	{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
		cv::gpu::GpuMat imgGpu(imgRoi);
		cv::gpu::GpuMat maskGpu(maskRoi);
		(*_gpuOrb.obj)(imgGpu, maskGpu, keypoints);
#else
		UERROR("Cannot use ORBGPU because OpenCV is not built with gpu module.");
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
		cv::cuda::GpuMat d_image(imgRoi);
		cv::cuda::GpuMat d_mask(maskRoi);
		try {
			_gpuOrb->detectAndCompute(d_image, d_mask, keypoints, cv::cuda::GpuMat(), false);
		} catch (cv::Exception& e) {
			const char* err_msg = e.what();
			UWARN("OpenCV exception caught: %s", err_msg);
		}
#endif
#endif
	}
	else
	{
		_orb->detect(imgRoi, keypoints, maskRoi);
	}

	return keypoints;
}

cv::Mat ORB::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(image.empty())
	{
		ULOGGER_ERROR("Image is null ?!?");
		return descriptors;
	}
	if(gpu_)
	{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
		cv::gpu::GpuMat imgGpu(image);
		cv::gpu::GpuMat descriptorsGPU;
		(*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
		// Download descriptors
		if (descriptorsGPU.empty())
			descriptors = cv::Mat();
		else
		{
			UASSERT(descriptorsGPU.type() == CV_32F);
			descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
			descriptorsGPU.download(descriptors);
		}
#else
		UERROR("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
		cv::cuda::GpuMat d_image(image);
		cv::cuda::GpuMat d_descriptors;
		try {
			_gpuOrb->detectAndCompute(d_image, cv::cuda::GpuMat(), keypoints, d_descriptors, true);
		} catch (cv::Exception& e) {
			const char* err_msg = e.what();
			UWARN("OpenCV exception caught: %s", err_msg);
		}
		// Download descriptors
		if (d_descriptors.empty())
			descriptors = cv::Mat();
		else
		{
			UASSERT(d_descriptors.type() == CV_32F || d_descriptors.type() == CV_8U);
			d_descriptors.download(descriptors);
		}
#endif
#endif
	}
	else
	{
		_orb->compute(image, keypoints, descriptors);
	}

	return descriptors;
}

//////////////////////////
//FAST
//////////////////////////
FAST::FAST(const ParametersMap & parameters) :
		threshold_(Parameters::defaultFASTThreshold()),
		nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
		gpu_(Parameters::defaultFASTGpu()),
		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
		minThreshold_(Parameters::defaultFASTMinThreshold()),
		maxThreshold_(Parameters::defaultFASTMaxThreshold()),
		gridRows_(Parameters::defaultFASTGridRows()),
		gridCols_(Parameters::defaultFASTGridCols()),
		fastCV_(Parameters::defaultFASTCV()),
		fastCVinit_(false),
		fastCVMaxFeatures_(10000),
		fastCVLastImageHeight_(0)
{
#ifdef RTABMAP_FASTCV
	char sVersion[128] = { 0 };
	fcvGetVersion(sVersion, 128);
	UINFO("fastcv version = %s", sVersion);
	int ix;
	if ((ix = fcvSetOperationMode(FASTCV_OP_PERFORMANCE)))
	{
		UERROR("fcvSetOperationMode return=%d, OpenCV FAST will be used instead!", ix);
		fastCV_ = 0;
	}
	else
	{
		fcvMemInit();

		if (!(fastCVCorners_ = (uint32_t*)fcvMemAlloc(fastCVMaxFeatures_ * sizeof(uint32_t) * 2, 16)) ||
			!(fastCVCornerScores_ = (uint32_t*)fcvMemAlloc( fastCVMaxFeatures_ * sizeof(uint32_t), 16 )))
		{
			UERROR("could not alloc fastcv mem, using opencv fast instead!");

			if (fastCVCorners_)
			{
				fcvMemFree(fastCVCorners_);
				fastCVCorners_ = NULL;
			}
			if (fastCVCornerScores_)
			{
				fcvMemFree(fastCVCornerScores_);
				fastCVCornerScores_ = NULL;
			}
		}
		else
		{
			fastCVinit_ = true;
		}
	}
	#endif
	parseParameters(parameters);
}

FAST::~FAST()
{
#ifdef RTABMAP_FASTCV
	if(fastCVinit_)
	{
		fcvMemDeInit();

		if (fastCVCorners_)
			fcvMemFree(fastCVCorners_);
		if (fastCVCornerScores_)
			fcvMemFree(fastCVCornerScores_);
		if (fastCVTempBuf_)
			fcvMemFree(fastCVTempBuf_);
	}
#endif
}

void FAST::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
	Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);

	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
	Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
	Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
	Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);

	Parameters::parse(parameters, Parameters::kFASTCV(), fastCV_);
	UASSERT(fastCV_ == 0 || fastCV_ == 9 || fastCV_ == 10);

	UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
	UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());

#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of FAST not available! Using CPU version instead...");
		gpu_ = false;
	}
#else
	if(gpu_)
	{
		UWARN("GPU version of FAST not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
		gpu_ = false;
	}
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
	if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
	{
		UWARN("GPU version of FAST not available! Using CPU version instead...");
		gpu_ = false;
	}
#else
	if(gpu_)
	{
		UWARN("GPU version of FAST not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
		gpu_ = false;
	}
#endif
#endif
	if(gpu_)
	{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
		_gpuFast = new CV_FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
#else
		UFATAL("not supposed to be here!");
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
		UFATAL("not implemented");
#endif
#endif
	}
	else
	{
#if CV_MAJOR_VERSION < 3
		if(gridRows_ > 0 && gridCols_ > 0)
		{
			UDEBUG("grid max features = %d", this->getMaxFeatures());
			cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
			_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, this->getMaxFeatures(), gridRows_, gridCols_));
		}
		else
		{
			if(gridRows_ > 0)
			{
				UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
						Parameters::kFASTGridRows().c_str(), gridRows_, Parameters::kFASTGridCols().c_str());
			}
			else if(gridCols_ > 0)
			{
				UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
						Parameters::kFASTGridCols().c_str(), gridCols_, Parameters::kFASTGridRows().c_str());
			}
			_fast = cv::Ptr<cv::FeatureDetector>(new CV_FAST(threshold_, nonmaxSuppression_));
		}
#else
		_fast = CV_FAST::create(threshold_, nonmaxSuppression_);
#endif
	}
}

bool FAST::isGpuAvailable() const
{
	// Not implemented
	return false;
}

std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	std::vector<cv::KeyPoint> keypoints;

#ifdef RTABMAP_FASTCV
	if(fastCV_>0)
	{
		// Note: mask not supported, it should be the inverse of the current mask used (0=where to extract)
		uint32_t nCorners = 0;

		UASSERT(fastCVCorners_ != NULL && fastCVCornerScores_ != NULL);
		if (nonmaxSuppression_)
		{
			if(fastCVTempBuf_==NULL || (fastCVTempBuf_!= NULL && fastCVLastImageHeight_!= image.rows))
			{
				if (fastCVTempBuf_)
				{
					fcvMemFree(fastCVTempBuf_);
					fastCVTempBuf_ = NULL;
				}
				if(!(fastCVTempBuf_ = (uint32_t*)fcvMemAlloc( (3*fastCVMaxFeatures_+image.rows+1)*4, 16 )))
				{
					UERROR("could not alloc fastcv mem for temp buf (%s=true)", Parameters::kFASTNonmaxSuppression().c_str());
					fastCVLastImageHeight_ = 0;
					return keypoints;
				}
				fastCVLastImageHeight_ = image.rows;
			}
		}

		// image.data should be 128 bits aligned
		UDEBUG("%dx%d (step=%d) thr=%d maxFeatures=%d", image.cols, image.rows, image.step1(), threshold_, fastCVMaxFeatures_);
		if(fastCV_ == 10)
		{
			fcvCornerFast10Scoreu8(image.data, image.cols, image.rows, 0, threshold_, 0, fastCVCorners_, fastCVCornerScores_, fastCVMaxFeatures_, &nCorners, nonmaxSuppression_?1:0, fastCVTempBuf_);
		}
		else
		{
			fcvCornerFast9Scoreu8_v2(image.data, image.cols, image.rows, image.step1(), threshold_, 0, fastCVCorners_, fastCVCornerScores_, fastCVMaxFeatures_, &nCorners, nonmaxSuppression_?1:0, fastCVTempBuf_);
		}
		UDEBUG("number of corners found = %d:", nCorners);
		keypoints.resize(nCorners);
		for (uint32_t i = 0; i < nCorners; i++)
		{
			keypoints[i].pt.x = fastCVCorners_[i * 2];
			keypoints[i].pt.y = fastCVCorners_[(i * 2) + 1];
			keypoints[i].size = 3;
			keypoints[i].response = fastCVCornerScores_[i];
		}

		if(this->getMaxFeatures() > 0)
		{
			this->limitKeypoints(keypoints, this->getMaxFeatures());
		}
		return keypoints;
	}
#endif

	if(fastCV_>0)
	{
		UWARN(  "RTAB-Map is not built with FastCV support. OpenCV's FAST is used instead. "
				"Please set %s to 0. This message will only appear once.",
				Parameters::kFASTCV().c_str());
		fastCV_ = 0;
	}

	cv::Mat imgRoi(image, roi);
	cv::Mat maskRoi;
	if(!mask.empty())
	{
		maskRoi = cv::Mat(mask, roi);
	}
	if(gpu_)
	{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
		cv::gpu::GpuMat imgGpu(imgRoi);
		cv::gpu::GpuMat maskGpu(maskRoi);
		(*_gpuFast.obj)(imgGpu, maskGpu, keypoints);
#else
		UERROR("Cannot use FAST GPU because OpenCV is not built with gpu module.");
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
		UFATAL("not implemented");
#endif
#endif
	}
	else
	{
		_fast->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
	}
	return keypoints;
}

//////////////////////////
//FAST-BRIEF
//////////////////////////
FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
	FAST(parameters),
	bytes_(Parameters::defaultBRIEFBytes())
{
	parseParameters(parameters);
}

FAST_BRIEF::~FAST_BRIEF()
{
}

void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
{
	FAST::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kBRIEFBytes(), 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;
}

//////////////////////////
//FAST-FREAK
//////////////////////////
FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
	FAST(parameters),
	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
	patternScale_(Parameters::defaultFREAKPatternScale()),
	nOctaves_(Parameters::defaultFREAKNOctaves())
{
	parseParameters(parameters);
}

FAST_FREAK::~FAST_FREAK()
{
}

void FAST_FREAK::parseParameters(const ParametersMap & parameters)
{
	FAST::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), 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;
}

//////////////////////////
//GFTT
//////////////////////////
GFTT::GFTT(const ParametersMap & parameters) :
		_qualityLevel(Parameters::defaultGFTTQualityLevel()),
		_minDistance(Parameters::defaultGFTTMinDistance()),
		_blockSize(Parameters::defaultGFTTBlockSize()),
		_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
		_k(Parameters::defaultGFTTK()),
		_gpu(Parameters::defaultGFTTGpu())
{
	parseParameters(parameters);
}

GFTT::~GFTT()
{
}

void GFTT::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
	Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
	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
	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
	}
}

bool GFTT::isGpuAvailable() const
{
#ifdef HAVE_OPENCV_CUDAIMGPROC
	return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
	return false;
#endif
}

std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	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())
	{
		maskRoi = cv::Mat(mask, roi);
	}

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

	if(!_useHarrisDetector && _qualityLevel>0.0)
	{
		std::vector<cv::KeyPoint> bestKeypoints;
		bestKeypoints.reserve(keypoints.size());
		for(size_t i=0; i<keypoints.size(); ++i)
		{
			if(keypoints[i].response > _qualityLevel)
			{
				bestKeypoints.push_back(keypoints[i]);
			}
		}
		
		return bestKeypoints;
	}
	return keypoints;
}

//////////////////////////
//FAST-BRIEF
//////////////////////////
GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
	GFTT(parameters),
	bytes_(Parameters::defaultBRIEFBytes())
{
	parseParameters(parameters);
}

GFTT_BRIEF::~GFTT_BRIEF()
{
}

void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
{
	GFTT::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kBRIEFBytes(), 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;
}

//////////////////////////
//GFTT-FREAK
//////////////////////////
GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
	GFTT(parameters),
	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
	patternScale_(Parameters::defaultFREAKPatternScale()),
	nOctaves_(Parameters::defaultFREAKNOctaves())
{
	parseParameters(parameters);
}

GFTT_FREAK::~GFTT_FREAK()
{
}

void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
{
	GFTT::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), 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;
}

//////////////////////////
//SURF-FREAK
//////////////////////////
SURF_FREAK::SURF_FREAK(const ParametersMap & parameters) :
	SURF(parameters),
	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
	patternScale_(Parameters::defaultFREAKPatternScale()),
	nOctaves_(Parameters::defaultFREAKNOctaves())
{
	parseParameters(parameters);
}

SURF_FREAK::~SURF_FREAK()
{
}

void SURF_FREAK::parseParameters(const ParametersMap & parameters)
{
	SURF::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), 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 SURF_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;
}

//////////////////////////
//GFTT-ORB
//////////////////////////
GFTT_ORB::GFTT_ORB(const ParametersMap & parameters) :
	GFTT(parameters),
	_orb(parameters)
{
	parseParameters(parameters);
}

GFTT_ORB::~GFTT_ORB()
{
}

void GFTT_ORB::parseParameters(const ParametersMap & parameters)
{
	GFTT::parseParameters(parameters);
	_orb.parseParameters(parameters);
}

cv::Mat GFTT_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	return _orb.generateDescriptors(image, keypoints);
}

//////////////////////////
//BRISK
//////////////////////////
BRISK::BRISK(const ParametersMap & parameters) :
	thresh_(Parameters::defaultBRISKThresh()),
	octaves_(Parameters::defaultBRISKOctaves()),
	patternScale_(Parameters::defaultBRISKPatternScale())
{
	parseParameters(parameters);
}

BRISK::~BRISK()
{
}

void BRISK::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
#if CV_MAJOR_VERSION > 4
#ifdef HAVE_OPENCV_XFEATURES2D
	brisk_ = CV_BRISK::create(thresh_, octaves_, patternScale_);
#else
	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so BRISK cannot be used!");
#endif
#elif 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 cv::Mat & mask)
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	std::vector<cv::KeyPoint> keypoints;
#if CV_MAJOR_VERSION < 5 || (CV_MAJOR_VERSION > 4 && defined(HAVE_OPENCV_XFEATURES2D))
	cv::Mat imgRoi(image, roi);
	cv::Mat maskRoi;
	if(!mask.empty())
	{
		maskRoi = cv::Mat(mask, roi);
	}
	brisk_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
#else
	UWARN("RTAB-Map is not built with BRISK feature support!");
#endif
	return keypoints;
}

cv::Mat BRISK::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 < 5 || (CV_MAJOR_VERSION > 4 && defined(HAVE_OPENCV_XFEATURES2D))
	brisk_->compute(image, keypoints, descriptors);
#else
	UWARN("RTAB-Map is not built with BRISK feature support!");
#endif
	return descriptors;
}

//////////////////////////
//KAZE
//////////////////////////
KAZE::KAZE(const ParametersMap & parameters) :
		extended_(Parameters::defaultKAZEExtended()),
		upright_(Parameters::defaultKAZEUpright()),
		threshold_(Parameters::defaultKAZEThreshold()),
		nOctaves_(Parameters::defaultKAZENOctaves()),
		nOctaveLayers_(Parameters::defaultKAZENOctaveLayers()),
		diffusivity_(Parameters::defaultKAZEDiffusivity())
{
	parseParameters(parameters);
}

KAZE::~KAZE()
{
}

void KAZE::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);
	
	Parameters::parse(parameters, Parameters::kKAZEExtended(), extended_);
	Parameters::parse(parameters, Parameters::kKAZEUpright(), upright_);
	Parameters::parse(parameters, Parameters::kKAZEThreshold(), threshold_);
	Parameters::parse(parameters, Parameters::kKAZENOctaves(), nOctaves_);
	Parameters::parse(parameters, Parameters::kKAZENOctaveLayers(), nOctaveLayers_);
	Parameters::parse(parameters, Parameters::kKAZEDiffusivity(), diffusivity_);

#if CV_MAJOR_VERSION > 4
#ifdef HAVE_OPENCV_XFEATURES2D
	kaze_ = CV_KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, (CV_KAZE::DiffusivityType)diffusivity_);
#else
	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so KAZE cannot be used!");
#endif
#elif CV_MAJOR_VERSION > 3
	kaze_ = CV_KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, (CV_KAZE::DiffusivityType)diffusivity_);
#elif CV_MAJOR_VERSION > 2
	kaze_ = CV_KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, diffusivity_);
#else
	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
#endif
}

std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	std::vector<cv::KeyPoint> keypoints;
#if (CV_MAJOR_VERSION > 2 && CV_MAJOR_VERSION < 5) || (CV_MAJOR_VERSION > 4 && defined(HAVE_OPENCV_XFEATURES2D))
	cv::Mat imgRoi(image, roi);
	cv::Mat maskRoi;
	if (!mask.empty())
	{
		maskRoi = cv::Mat(mask, roi);
	}
	kaze_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
#else
	UWARN("RTAB-Map is not built with Kaze feature support!");
#endif
	return keypoints;
}

cv::Mat KAZE::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 > 2 && CV_MAJOR_VERSION < 5) || (CV_MAJOR_VERSION > 4 && defined(HAVE_OPENCV_XFEATURES2D))
	kaze_->compute(image, keypoints, descriptors);
#else
	UWARN("RTAB-Map is not built with Kaze feature support!");
#endif
	return descriptors;
}

//////////////////////////
//ORBOctree
//////////////////////////
ORBOctree::ORBOctree(const ParametersMap & parameters) :
		scaleFactor_(Parameters::defaultORBScaleFactor()),
		nLevels_(Parameters::defaultORBNLevels()),
		patchSize_(Parameters::defaultORBPatchSize()),
		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
		fastThreshold_(Parameters::defaultFASTThreshold()),
		fastMinThreshold_(Parameters::defaultFASTMinThreshold())
{
	parseParameters(parameters);
}

ORBOctree::~ORBOctree()
{
}

void ORBOctree::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
	Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);

	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), fastMinThreshold_);

#ifdef RTABMAP_ORB_OCTREE
	_orb = cv::Ptr<ORBextractor>(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_, patchSize_, edgeThreshold_));
#else
	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
#endif
}

std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
	std::vector<cv::KeyPoint> keypoints;
	descriptors_ = cv::Mat();
#ifdef RTABMAP_ORB_OCTREE
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	cv::Mat imgRoi(image, roi);
	cv::Mat maskRoi;
	if(!mask.empty())
	{
		maskRoi = cv::Mat(mask, roi);
	}

	(*_orb)(imgRoi, maskRoi, keypoints, descriptors_);

	// OrbOctree ignores the mask, so we have to apply it manually here
	if(!keypoints.empty() && !maskRoi.empty())
	{
		std::vector<cv::KeyPoint> validKeypoints;
		validKeypoints.reserve(keypoints.size());
		cv::Mat validDescriptors;
		for(size_t i=0; i<keypoints.size(); ++i)
		{
			if(maskRoi.at<unsigned char>(keypoints[i].pt.y+roi.y, keypoints[i].pt.x+roi.x) != 0)
			{
				validKeypoints.push_back(keypoints[i]);
				validDescriptors.push_back(descriptors_.row(i));
			}
		}
		keypoints = validKeypoints;
		descriptors_ = validDescriptors;
	}

	if((int)keypoints.size() > this->getMaxFeatures())
	{
		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), roi.size(), this->getSSC());
	}
#else
	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
#endif
	return keypoints;
}

cv::Mat ORBOctree::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
#ifdef RTABMAP_ORB_OCTREE
	UASSERT_MSG((int)keypoints.size() == descriptors_.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors_.rows).c_str());
#else
	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
#endif
	return descriptors_;
}

//////////////////////////
//SuperPointTorch
//////////////////////////
SuperPointTorch::SuperPointTorch(const ParametersMap & parameters) :
		path_(Parameters::defaultSuperPointModelPath()),
		threshold_(Parameters::defaultSuperPointThreshold()),
		nms_(Parameters::defaultSuperPointNMS()),
		minDistance_(Parameters::defaultSuperPointNMSRadius()),
		cuda_(Parameters::defaultSuperPointCuda())
{
	parseParameters(parameters);
}

SuperPointTorch::~SuperPointTorch()
{
}

bool SuperPointTorch::isGpuAvailable() const
{
#ifdef RTABMAP_TORCH
	return torch::cuda::is_available();
#else
	return false;
#endif
}

void SuperPointTorch::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

	std::string previousPath = path_;
#ifdef RTABMAP_TORCH
	bool previousCuda = cuda_;
#endif
	Parameters::parse(parameters, Parameters::kSuperPointModelPath(), path_);
	Parameters::parse(parameters, Parameters::kSuperPointThreshold(), threshold_);
	Parameters::parse(parameters, Parameters::kSuperPointNMS(), nms_);
	Parameters::parse(parameters, Parameters::kSuperPointNMSRadius(), minDistance_);
	Parameters::parse(parameters, Parameters::kSuperPointCuda(), cuda_);

#ifdef RTABMAP_TORCH
	if(superPoint_.get() == 0 || path_.compare(previousPath) != 0 || previousCuda != cuda_)
	{
		superPoint_ = cv::Ptr<SPDetector>(new SPDetector(path_, threshold_, nms_, minDistance_, cuda_));
	}
	else
	{
		superPoint_->setThreshold(threshold_);
		superPoint_->SetNMS(nms_);
		superPoint_->setMinDistance(minDistance_);
	}
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
#endif
}

std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
#ifdef RTABMAP_TORCH
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	if(roi.x!=0 || roi.y !=0 || roi.width!=image.cols || roi.height!=image.rows)
	{
		UERROR("SuperPoint: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
				roi.x, roi.y, roi.width, roi.height,
				Parameters::kKpRoiRatios().c_str(),
				Parameters::kVisRoiRatios().c_str(),
				Parameters::kVisGridRows().c_str(),
				Parameters::kVisGridCols().c_str(),
				Parameters::kKpGridRows().c_str(),
				Parameters::kKpGridCols().c_str());
		return std::vector<cv::KeyPoint>();
	}
	return superPoint_->detect(image, mask);
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
	return std::vector<cv::KeyPoint>();
#endif
}

cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
#ifdef RTABMAP_TORCH
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	cv::Mat descriptors;
	if(!keypoints.empty())
	{
		descriptors = superPoint_->compute(keypoints);
		if(descriptors.empty())
		{
			// superpoint may have been reset between keypoint detection and now,
			// re-detect features to re-inialize the descriptors matrix, then
			// re-extract descriptors with original keypoints.
			UWARN("Re-initializing superpoint on that image to extract descriptors");
			if(!superPoint_->detect(image).empty())
			{
				descriptors = superPoint_->compute(keypoints);
				if(descriptors.rows == (int)keypoints.size())
				{
					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
				}
			}
			else
			{
				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
			}
		}
	}
	return descriptors;
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
	return cv::Mat();
#endif
}


//////////////////////////
//SuperPointRpautrat
//////////////////////////
SuperPointRpautrat::SuperPointRpautrat(const ParametersMap & parameters) :
		superpointWeightsPath_(Parameters::defaultSuperPointRpautratWeightsPath()),
		superpointModelPath_(Parameters::defaultSuperPointRpautratModelPath()),
		outputDir_(""),
		threshold_(Parameters::defaultSuperPointRpautratThreshold()),
		nms_(Parameters::defaultSuperPointRpautratNMS()),
		minDistance_(Parameters::defaultSuperPointRpautratNMSRadius()),
		cuda_(Parameters::defaultSuperPointRpautratCuda())
{
	parseParameters(parameters);
}

SuperPointRpautrat::~SuperPointRpautrat()
{
}

bool SuperPointRpautrat::isGpuAvailable() const
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
	return torch::cuda::is_available();
#else
	return false;
#endif
}

void SuperPointRpautrat::parseParameters(const ParametersMap & parameters)
{
	Feature2D::parseParameters(parameters);

#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
	std::string previousWeightsPath = superpointWeightsPath_;
	std::string previousModelPath = superpointModelPath_;
	bool previousCuda = cuda_;
	float previousThreshold = threshold_;
	bool previousNms = nms_;
	int previousMinDistance = minDistance_;
	
	Parameters::parse(parameters, Parameters::kSuperPointRpautratWeightsPath(), superpointWeightsPath_);
	Parameters::parse(parameters, Parameters::kSuperPointRpautratModelPath(), superpointModelPath_);
	Parameters::parse(parameters, Parameters::kSuperPointRpautratThreshold(), threshold_);
	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMS(), nms_);
	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMSRadius(), minDistance_);
	Parameters::parse(parameters, Parameters::kSuperPointRpautratCuda(), cuda_);
	Parameters::parse(parameters, Parameters::kRtabmapWorkingDirectory(), outputDir_);

	// If working directory is not set, use the default
	if(outputDir_.empty())
	{
		outputDir_ = Parameters::createDefaultWorkingDirectory();
	}

	// Reinitialize detector if model-affecting parameters changed
	if(superPoint_.get() == 0 || 
	   superpointWeightsPath_.compare(previousWeightsPath) != 0 || 
	   superpointModelPath_.compare(previousModelPath) != 0 ||
	   previousCuda != cuda_ ||
	   previousThreshold != threshold_ ||
	   previousNms != nms_ ||
	   previousMinDistance != minDistance_)
	{
		superPoint_ = cv::Ptr<SPDetectorRpautrat>(new SPDetectorRpautrat(superpointWeightsPath_, superpointModelPath_, outputDir_, threshold_, nms_, minDistance_, cuda_, this->getMaxFeatures(), this->getSSC()));
	}
	else if(superPoint_.get() != 0)
	{
		// Update post-processing parameters without reinitializing
		superPoint_->setMaxFeatures(this->getMaxFeatures());
		superPoint_->setSSC(this->getSSC());
	}
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
#endif
}

std::vector<cv::KeyPoint> SuperPointRpautrat::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	if(roi.x!=0 || roi.y !=0 || roi.width!=image.cols || roi.height!=image.rows)
	{
		UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
				roi.x, roi.y, roi.width, roi.height,
				Parameters::kKpRoiRatios().c_str(),
				Parameters::kVisRoiRatios().c_str(),
				Parameters::kVisGridRows().c_str(),
				Parameters::kVisGridCols().c_str(),
				Parameters::kKpGridRows().c_str(),
				Parameters::kKpGridCols().c_str());
		return std::vector<cv::KeyPoint>();
	}
	return superPoint_->detect(image, mask);
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
	return std::vector<cv::KeyPoint>();
#endif
}

cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
	cv::Mat descriptors;
	if(!keypoints.empty())
	{
		descriptors = superPoint_->compute(keypoints);
		if(descriptors.empty())
		{
			// superpoint may have been reset between keypoint detection and now,
			// re-detect features to re-inialize the descriptors matrix, then
			// re-extract descriptors with original keypoints.
			UWARN("Re-initializing superpoint on that image to extract descriptors");
			if(!superPoint_->detect(image).empty())
			{
				descriptors = superPoint_->compute(keypoints);
				if(descriptors.rows == (int)keypoints.size())
				{
					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
				}
			}
			else
			{
				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
			}
		}
	}
	return descriptors;
#else
	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
	return cv::Mat();
#endif
}


//////////////////////////
//GFTT-DAISY
//////////////////////////
GFTT_DAISY::GFTT_DAISY(const ParametersMap & parameters) :
	GFTT(parameters),
	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
	patternScale_(Parameters::defaultFREAKPatternScale()),
	nOctaves_(Parameters::defaultFREAKNOctaves())
{
	parseParameters(parameters);
}

GFTT_DAISY::~GFTT_DAISY()
{
}

void GFTT_DAISY::parseParameters(const ParametersMap & parameters)
{
	GFTT::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);

#ifdef HAVE_OPENCV_XFEATURES2D
	_daisy = CV_DAISY::create();
#else
	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
#endif
}

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

//////////////////////////
//SURF-DAISY
//////////////////////////
SURF_DAISY::SURF_DAISY(const ParametersMap & parameters) :
	SURF(parameters),
	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
	patternScale_(Parameters::defaultFREAKPatternScale()),
	nOctaves_(Parameters::defaultFREAKNOctaves())
{
	parseParameters(parameters);
}

SURF_DAISY::~SURF_DAISY()
{
}

void SURF_DAISY::parseParameters(const ParametersMap & parameters)
{
	SURF::parseParameters(parameters);

	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);

#ifdef HAVE_OPENCV_XFEATURES2D
	_daisy = CV_DAISY::create();
#else
	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
#endif
}

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

}