Add ANMS (SSC method) (#1276)

corelib/include/rtabmap/core/Features2d.h

@@ -192,16 +192,17 @@ public:
 			const cv::Mat & disparity,
 			float minDisparity);
 
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
 	int getMaxFeatures() const {return maxFeatures_;}
+	bool getSSC() const {return SSC_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
 	int getGridRows() const {return gridRows_;}
@@ -234,6 +235,7 @@ private:
 private:
 	ParametersMap parameters_;
 	int maxFeatures_;
+	bool SSC_;
 	float _maxDepth; // 0=inf
 	float _minDepth;
 	std::vector<float> _roiRatios; // size 4

corelib/include/rtabmap/core/Memory.h

@@ -334,6 +334,7 @@ private:
 	bool _rotateImagesUpsideUp;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
+	bool _visSSC;
 	bool _imagesAlreadyRectified;
 	bool _rectifyOnlyFeatures;
 	bool _covOffDiagonalIgnored;

corelib/include/rtabmap/core/Parameters.h

@@ -244,6 +244,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+    RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
@@ -693,6 +694,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
+    RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");

corelib/include/rtabmap/core/util2d.h

@@ -164,6 +164,9 @@ void RTABMAP_CORE_EXPORT NMS(
 		cv::Mat & descriptorsOut,
 		int border, int dist_thresh, int img_width, int img_height);
 
+std::vector<int> RTABMAP_CORE_EXPORT SSC(
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows);
+
 /**
  * @brief Rotate images and camera model so that the top of the image is up.
  *

corelib/src/Features2d.cpp

@@ -268,70 +268,111 @@ void Feature2D::filterKeypointsByDisparity(
 	}
 }
 
-void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize, bool ssc)
 {
 	cv::Mat descriptors;
-	limitKeypoints(keypoints, descriptors, maxKeypoints);
+	limitKeypoints(keypoints, descriptors, maxKeypoints, imageSize, ssc);
 }
 
-void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+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);
+	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)
+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;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", 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
-		int removed = (int)hessianMap.size()-maxKeypoints;
-		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		int removed;
+		std::vector<cv::KeyPoint> kptsTmp;
 		std::vector<cv::Point3f> kpts3DTmp;
-		if(!keypoints3D.empty())
-		{
-			kpts3DTmp.resize(maxKeypoints);
-		}
 		cv::Mat descriptorsTmp;
-		if(descriptors.rows)
+		if(ssc)
 		{
-			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())
+			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			static constexpr float tolerance = 0.1;
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			removed = keypoints.size()-ResultVec.size();
+			// retrieve final keypoints
+			kptsTmp.resize(ResultVec.size());
+			if(!keypoints3D.empty())
 			{
-				kpts3DTmp[k] = keypoints3D[iter->second];
+				kpts3DTmp.resize(ResultVec.size());
 			}
 			if(descriptors.rows)
 			{
-				if(descriptors.type() == CV_32FC1)
+				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())
 				{
-					memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+					kpts3DTmp[k] = keypoints3D[ResultVec[k]];
 				}
-				else
+				if(descriptors.rows)
 				{
-					memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
+					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));
+					}
 				}
 			}
 		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
+		else
+		{
+			ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", 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;
@@ -342,31 +383,46 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 	}
 }
 
-void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
+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;
-		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
-		int removed = (int)hessianMap.size()-maxKeypoints;
-		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-		inliers.resize(keypoints.size(), false);
 		float minimumHessian = 0.0f;
-		for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
+		int removed;
+		inliers.resize(keypoints.size(), false);
+		if(ssc)
 		{
-			inliers[iter->second] = true;
-			minimumHessian = iter->first;
+			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			static constexpr float tolerance = 0.1;
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			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", 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, maxKeypoints, minimumHessian);
 		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
@@ -378,7 +434,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	}
 }
 
-void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
+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)
 	{
@@ -406,7 +462,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	for(size_t i=0; i<keypointsPerCell.size(); ++i)
 	{
 		std::vector<bool> inliersCell;
-		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
+		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell, cv::Size(colSize, rowSize), ssc);
 		for(size_t j=0; j<inliersCell.size(); ++j)
 		{
 			if(inliersCell[j])
@@ -432,6 +488,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 /////////////////////
 Feature2D::Feature2D(const ParametersMap & parameters) :
 		maxFeatures_(Parameters::defaultKpMaxFeatures()),
+		SSC_(Parameters::defaultKpSSC()),
 		_maxDepth(Parameters::defaultKpMaxDepth()),
 		_minDepth(Parameters::defaultKpMinDepth()),
 		_roiRatios(std::vector<float>(4, 0.0f)),
@@ -453,6 +510,7 @@ 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);
@@ -736,7 +794,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
 			if (this->getType() != Feature2D::Type::kFeaturePyDetector)
 			{
-				limitKeypoints(subKeypoints, maxFeatures);
+				limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
 			}
 			if(roi.x || roi.y)
 			{
@@ -2142,7 +2200,7 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image
 
 	if((int)keypoints.size() > this->getMaxFeatures())
 	{
-		limitKeypoints(keypoints, descriptors_, 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!");

corelib/src/Memory.cpp

@@ -111,6 +111,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
+	_visSSC(Parameters::defaultVisSSC()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
 	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
@@ -603,6 +604,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
 	Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
 	Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
+	Parameters::parse(params, Parameters::kVisSSC(), _visSSC);
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
 	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
 	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -5131,9 +5133,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
 
 		int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
+		bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
 		if((int)keypoints.size() > maxFeatures)
 		{
-			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
+			if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
+			else
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -5349,22 +5355,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			int inliersCount = 0;
 			if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
 				(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
-				 data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
+					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
 			{
-				Feature2D::limitKeypoints(keypoints,
-						inliers,
-						_feature2D->getMaxFeatures(),
-						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
-						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
-						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
-						_feature2D->getGridRows(), _feature2D->getGridCols());
-				for(size_t i=0; i<inliers.size(); ++i)
-				{
-					if(inliers[i])
-					{
-						++inliersCount;
-					}
-				}
+				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+					decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+					decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+					data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+					_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
 			}
 			else
 			{
@@ -5373,8 +5370,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
 							Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
 				}
-				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
-				inliersCount = _feature2D->getMaxFeatures();
+				if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
+					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+				{
+					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+						_feature2D->getSSC());
+				}
+				else
+				{
+					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+				}
+			}
+			for(size_t i=0; i<inliers.size(); ++i)
+			{
+				if(inliers[i])
+					++inliersCount;
 			}
 
 			descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());

corelib/src/RegistrationVis.cpp

@@ -102,6 +102,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), _featureParameters.at(Parameters::kVisSSC())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -234,6 +235,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
 	}
+	if(uContains(parameters, Parameters::kVisSSC()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
+	}
 	if(uContains(parameters, Parameters::kVisMaxDepth()))
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));

corelib/src/SensorCaptureThread.cpp

@@ -241,6 +241,7 @@ void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameter
 	ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
 	uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
 	uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
+	uInsert(params, ParametersPair(Parameters::kKpSSC(), uValue(params, Parameters::kVisSSC(), defaultParams.at(Parameters::kVisSSC()))));
 	uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
 	uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
 	uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));

corelib/src/util2d.cpp

@@ -2202,6 +2202,83 @@ void NMS(
     }
 }
 
+std::vector<int> SSC(
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
+{
+	// several temp expression variables to simplify solution equation
+	int exp1 = rows + cols + 2*maxKeypoints;
+	long long exp2 = ((long long)4*cols + (long long)4*maxKeypoints + (long long)4*rows*maxKeypoints + (long long)rows*rows + (long long)cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*maxKeypoints);
+	double exp3 = sqrt(exp2);
+	double exp4 = maxKeypoints - 1;
+
+	double sol1 = -round((exp1 + exp3) / exp4); // first solution
+	double sol2 = -round((exp1 - exp3) / exp4); // second solution
+
+	// binary search range initialization with positive solution
+	int high = (sol1 > sol2) ? sol1 : sol2;
+	int low = floor(sqrt((double)keypoints.size() / maxKeypoints));
+	low = std::max(1, low);
+
+	int width;
+	int prevWidth = -1;
+
+	unsigned int Kmin = round(maxKeypoints - (maxKeypoints * tolerance));
+	unsigned int Kmax = round(maxKeypoints + (maxKeypoints * tolerance));
+
+	std::vector<int> ResultVec, result;
+	result.reserve(keypoints.size());
+
+	bool complete = false;
+	while(!complete)
+	{
+		width = low + (high - low) / 2;
+		if(width==prevWidth || low>high) // needed to reassure the same radius is not repeated again
+		{
+			ResultVec = result; // return the keypoints from the previous iteration
+			break;
+		}
+		result.clear();
+		double c = (double)width / 2.0; // initializing Grid
+		int numCellCols = floor(cols / c);
+		int numCellRows = floor(rows / c);
+		std::vector<std::vector<bool>> coveredVec(numCellRows+1, std::vector<bool>(numCellCols+1, false));
+
+		for(unsigned int i=0; i<keypoints.size(); ++i)
+		{
+			int row = floor(keypoints[i].pt.y / c); // get position of the cell current point is located at
+			int col = floor(keypoints[i].pt.x / c);
+			if(coveredVec[row][col] == false) // if the cell is not covered
+			{
+				result.push_back(i);
+				int rowMin = ((row - floor(width / c)) >= 0) ? (row - floor(width / c)) : 0; // get range which current radius is covering
+				int rowMax = ((row + floor(width / c)) <= numCellRows) ? (row + floor(width / c)) : numCellRows;
+				int colMin = ((col - floor(width / c)) >= 0) ? (col - floor(width / c)) : 0;
+				int colMax = ((col + floor(width / c)) <= numCellCols) ? (col + floor(width / c)) : numCellCols;
+				for(int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov)
+				{
+					for(int colToCov=colMin; colToCov<=colMax; ++colToCov)
+					{
+						if(!coveredVec[rowToCov][colToCov])
+							coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width
+					}
+				}
+			}
+		}
+
+		if(result.size() >= Kmin && result.size() <= Kmax) // solution found
+		{
+			ResultVec = result;
+			complete = true;
+		}
+		else if(result.size() < Kmin)
+			high = width - 1; // update binary search range
+		else
+			low = width + 1;
+		prevWidth = width;
+	}
+	return ResultVec;
+}
+
 void rotateImagesUpsideUpIfNecessary(
 	CameraModel & model,
 	cv::Mat & rgb,