Sort keypoints before SSC (#1364)

* Sort keypoints before SSC

* avoid using vector<bool>

corelib/include/rtabmap/core/Features2d.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
+#include <numeric>
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/SensorData.h"
 

corelib/include/rtabmap/core/util2d.h

@@ -165,7 +165,7 @@ void RTABMAP_CORE_EXPORT NMS(
 		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);
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx = {});
 
 /**
  * @brief Rotate images and camera model so that the top of the image is up.

corelib/src/Features2d.cpp

@@ -301,8 +301,24 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		if(ssc)
 		{
 			ULOGGER_DEBUG("too much words (%d), removing words with SSC", 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);
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height, indx);
 			removed = keypoints.size()-ResultVec.size();
 			// retrieve final keypoints
 			kptsTmp.resize(ResultVec.size());
@@ -401,8 +417,24 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 		if(ssc)
 		{
 			ULOGGER_DEBUG("too much words (%d), removing words with SSC", 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);
+			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)
 			{

corelib/src/util2d.cpp

@@ -2203,8 +2203,10 @@ void NMS(
 }
 
 std::vector<int> SSC(
-	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx)
 {
+	bool useIndx = keypoints.size() == indx.size();
+
 	// 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);
@@ -2241,27 +2243,20 @@ std::vector<int> SSC(
 		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));
+		cv::Mat coveredMask = cv::Mat::zeros(numCellRows + 1, numCellCols + 1, CV_8UC1);
 
 		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 row = floor(keypoints[useIndx?indx[i]:i].pt.y / c); // get position of the cell current point is located at
-			int col = floor(keypoints[i].pt.x / c);
+			int col = floor(keypoints[useIndx?indx[i]:i].pt.x / c);
-			if(coveredVec[row][col] == false) // if the cell is not covered
+			if(!coveredMask.at<uchar>(row, col)) // if the cell is not covered
 			{
-				result.push_back(i);
+				result.push_back(useIndx?indx[i]: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)
+				coveredMask(cv::Range(rowMin, rowMax + 1), cv::Range(colMin, colMax + 1)) = 255; // cover cells within the square bounding box with width
-				{
-					for(int colToCov=colMin; colToCov<=colMax; ++colToCov)
-					{
-						if(!coveredVec[rowToCov][colToCov])
-							coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width
-					}
-				}
 			}
 		}