move NMS to util2d

corelib/src/util2d.cpp

@@ -2093,6 +2093,118 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
 	}
 }
 
+void NMS(
+		const std::vector<cv::KeyPoint> & ptsIn,
+		const cv::Mat & descriptorsIn,
+		std::vector<cv::KeyPoint> & ptsOut,
+		cv::Mat & descriptorsOut,
+        int border, int dist_thresh, int img_width, int img_height)
+{
+    std::vector<cv::Point2f> pts_raw;
+
+    for (size_t i = 0; i < ptsIn.size(); i++)
+    {
+		int u = (int) ptsIn[i].pt.x;
+		int v = (int) ptsIn[i].pt.y;
+
+		pts_raw.emplace_back(cv::Point2f(u, v));
+	}
+
+    //Grid Value Legend:
+    //    255  : Kept.
+    //     0   : Empty or suppressed.
+    //    100  : To be processed (converted to either kept or suppressed).
+    cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1);
+    cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1);
+
+    cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1);
+
+    grid.setTo(0);
+    inds.setTo(0);
+    confidence.setTo(0);
+
+    for (size_t i = 0; i < pts_raw.size(); i++)
+    {
+        int uu = (int) pts_raw[i].x;
+        int vv = (int) pts_raw[i].y;
+
+        grid.at<unsigned char>(vv, uu) = 100;
+        inds.at<unsigned short>(vv, uu) = i;
+
+        confidence.at<float>(vv, uu) = ptsIn[i].response;
+    }
+
+    // debug
+    //cv::Mat confidenceVis = confidence.clone() * 255;
+    //confidenceVis.convertTo(confidenceVis, CV_8UC1);
+    //cv::imwrite("confidence.bmp", confidenceVis);
+    //cv::imwrite("grid_in.bmp", grid);
+
+    cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0);
+
+    for (size_t i = 0; i < pts_raw.size(); i++)
+    {
+    	// account for top left padding
+        int uu = (int) pts_raw[i].x + dist_thresh;
+        int vv = (int) pts_raw[i].y + dist_thresh;
+        float c = confidence.at<float>(vv-dist_thresh, uu-dist_thresh);
+
+        if (grid.at<unsigned char>(vv, uu) == 100)  // If not yet suppressed.
+        {
+			for(int k = -dist_thresh; k < (dist_thresh+1); k++)
+			{
+				for(int j = -dist_thresh; j < (dist_thresh+1); j++)
+				{
+					if(j==0 && k==0)
+						continue;
+
+					if ( confidence.at<float>(vv + k - dist_thresh, uu + j - dist_thresh) <= c )
+					{
+						grid.at<unsigned char>(vv + k, uu + j) = 0;
+					}
+				}
+			}
+			grid.at<unsigned char>(vv, uu) = 255;
+        }
+    }
+
+    size_t valid_cnt = 0;
+    std::vector<int> select_indice;
+
+    grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height));
+
+    //debug
+    //cv::imwrite("grid_nms.bmp", grid);
+
+    for (int v = 0; v < img_height; v++)
+    {
+        for (int u = 0; u < img_width; u++)
+        {
+            if (grid.at<unsigned char>(v,u) == 255)
+            {
+                int select_ind = (int) inds.at<unsigned short>(v, u);
+				ptsOut.emplace_back(ptsIn[select_ind]);
+                select_indice.emplace_back(select_ind);
+                valid_cnt++;
+            }
+        }
+    }
+
+    if(!descriptorsIn.empty())
+    {
+    	UASSERT(descriptorsIn.rows == (int)ptsIn.size());
+		descriptorsOut.create(select_indice.size(), 256, CV_32F);
+
+		for (size_t i=0; i<select_indice.size(); i++)
+		{
+			for (int j=0; j < 256; j++)
+			{
+				descriptorsOut.at<float>(i, j) = descriptorsIn.at<float>(select_indice[i], j);
+			}
+		}
+    }
+}
+
 }
 
 }