Improved ANMS to consider descriptors

src/CMakeLists.txt

@@ -83,6 +83,7 @@ SET(sources
 	matchers/flannbased/matcher_flannbased.cpp
 	matchers/flannbased/matcher_flannbased_load_yaml.cpp
 	algorithms/algorithm_base.cpp
+	algorithms/anms/anms.cpp
 	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp
 	algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp
 	algorithms/trackfeatures/alg_trackfeatures.cpp

src/algorithms/anms/anms.cpp

+#include "anms.h"
+
+namespace vision_utils {
+
+vector<cv::KeyPoint> TopN(vector<cv::KeyPoint> keyPoints, int numRetPoints)
+{
+    vector<cv::KeyPoint> kp;
+    for (int i = 0; i < numRetPoints; i++) kp.push_back(keyPoints[i]); //simply extracting numRetPoints keyPoints
+
+    return kp;
+}
+
+vector<cv::KeyPoint> BrownANMS(vector<cv::KeyPoint> keyPoints, int numRetPoints)
+{
+    vector<pair<float,int> > results;
+    results.push_back(make_pair(FLT_MAX,0));
+    for (unsigned int i = 1;i<keyPoints.size();++i){ //for every keyPoint we get the min distance to the previously visited keyPoints
+        float minDist = FLT_MAX;
+        for (unsigned int j=0;j<i;++j){
+            float exp1 = (keyPoints[j].pt.x-keyPoints[i].pt.x);
+            float exp2 = (keyPoints[j].pt.y-keyPoints[i].pt.y);
+            float curDist = sqrt(exp1*exp1+exp2*exp2);
+            minDist = min(curDist,minDist);
+        }
+        results.push_back(make_pair(minDist,i));
+    }
+    sort(results.begin(),results.end(),sort_pred()); //sorting by radius
+    vector<cv::KeyPoint> kp;
+    for (int i=0;i<numRetPoints;++i) kp.push_back(keyPoints[results[i].second]); //extracting numRetPoints keyPoints
+
+    return kp;
+}
+
+vector<cv::KeyPoint> Sdc(vector<cv::KeyPoint> keyPoints, int numRetPoints, float tolerance, int cols, int rows)
+{
+    double eps_var = 0.25; //this parameter is chosen to be the most optimal in the original paper
+
+    int low = 1; int high = cols; //binary search range initialization
+    int radius;
+    int prevradius = -1;
+
+    vector<int> ResultVec;
+    bool complete = false;
+    unsigned int K = numRetPoints;
+    unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
+
+    vector<int> result; result.reserve(keyPoints.size());
+    while(!complete){
+        radius = low+(high-low)/2;
+        if (radius == prevradius || 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 = eps_var*radius/sqrt(2); //initializing Grid
+        int numCellCols = floor(cols/c);
+        int numCellRows = floor(rows/c);
+        vector<vector<bool> > coveredVec(numCellRows+1,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(radius/c))>=0)? (row-floor(radius/c)) : 0; //get range which current radius is covering
+                int rowMax = ((row+floor(radius/c))<=numCellRows)? (row+floor(radius/c)) : numCellRows;
+                int colMin = ((col-floor(radius/c))>=0)? (col-floor(radius/c)) : 0;
+                int colMax = ((col+floor(radius/c))<=numCellCols)? (col+floor(radius/c)) : numCellCols;
+                for (int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov){
+                    for (int colToCov=colMin ; colToCov<=colMax; ++colToCov){
+                        double dist = sqrt((rowToCov-row)*(rowToCov-row) + (colToCov-col)*(colToCov-col));
+                        if (dist<=((double)radius)/c) coveredVec[rowToCov][colToCov] = true; //check the distance to every cell
+                    }
+                }
+            }
+        }
+        if (result.size()>=Kmin && result.size()<=Kmax){ //solution found
+            ResultVec = result;
+            complete = true;
+        }
+        else if (result.size()<Kmin) high = radius-1; //update binary search range
+        else low = radius+1;
+    }
+    // retrieve final keypoints
+    vector<cv::KeyPoint> kp;
+    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
+
+    return kp;
+}
+
+vector<cv::KeyPoint> KdTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance,int cols,int rows)
+{
+    // several temp expression variables to simplify solution equation
+    int exp1 = rows + cols + 2*numRetPoints;
+    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
+    double exp3 = sqrt(exp2);
+    double exp4 = (2*(numRetPoints - 1));
+
+    double sol1 = -round((exp1+exp3)/exp4); // first solution
+    double sol2 = -round((exp1-exp3)/exp4); // second solution
+
+    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
+    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
+
+
+    PointCloud<int> cloud; //creating k-d tree with keypoints
+    generatePointCloud(cloud,keyPoints);
+    typedef KDTreeSingleIndexAdaptor< L2_Simple_Adaptor<int, PointCloud<int> > , PointCloud<int>, 2> my_kd_tree_t;
+    my_kd_tree_t   index(2, cloud, KDTreeSingleIndexAdaptorParams(25 /* max leaf */) );
+    index.buildIndex();
+
+    bool complete = false;
+    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
+    vector<int> ResultVec;
+    int radius; int prevradius = -1;
+
+    vector<int> result; result.reserve(keyPoints.size());
+    while(!complete){
+        vector<bool> Included(keyPoints.size(),true);
+        radius = low+(high-low)/2;
+        if (radius == prevradius || low>high) { //needed to reassure the same radius is not repeated again
+            ResultVec = result; //return the keypoints from the previous iteration
+            break;
+        }
+        result.clear();
+
+        for (unsigned int i=0;i<keyPoints.size();++i){
+            if (Included[i]==true){
+                Included[i] = false;
+                result.push_back(i);
+                const int search_radius = static_cast<int>(radius*radius);
+                vector<pair<size_t,int> >   ret_matches;
+                nanoflann::SearchParams params;
+                const int query_pt[2] = { (int)keyPoints[i].pt.x, (int)keyPoints[i].pt.y};
+                const size_t nMatches = index.radiusSearch(&query_pt[0],search_radius, ret_matches, params);
+
+                for (size_t nmIdx=0; nmIdx<nMatches; nmIdx++) {
+                    if (Included[ret_matches[nmIdx].first]) Included[ret_matches[nmIdx].first] = false;
+                }
+            }
+        }
+
+        if (result.size()>=Kmin && result.size()<=Kmax){ //solution found
+            ResultVec = result;
+            complete = true;
+        }
+        else if (result.size()<Kmin) high = radius-1; //update binary search range
+        else low = radius+1;
+
+        prevradius = radius;
+    }
+
+    // retrieve final keypoints
+    vector<cv::KeyPoint> kp;
+    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
+
+    return kp;
+}
+
+vector<cv::KeyPoint> RangeTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance, int cols, int rows)
+{
+    // several temp expression variables to simplify solution equation
+    int exp1 = rows + cols + 2*numRetPoints;
+    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
+    double exp3 = sqrt(exp2);
+    double exp4 = (2*(numRetPoints - 1));
+
+    double sol1 = -round((exp1+exp3)/exp4); // first solution
+    double sol2 = -round((exp1-exp3)/exp4); // second solution
+
+    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
+    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
+
+    rangetree<u16, u16> treeANMS(keyPoints.size(), keyPoints.size()); //creating range tree with keypoints
+    for (unsigned int i = 0; i < keyPoints.size(); i++) treeANMS.add(keyPoints[i].pt.x, keyPoints[i].pt.y, (u16 *)(intptr_t)i);
+    treeANMS.finalize();
+
+    bool complete = false;
+    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
+    vector<int> ResultVec;
+    int width;
+    int prevwidth = -1;
+
+    vector<int> result; result.reserve(keyPoints.size());
+    while(!complete){
+        vector<bool> Included(keyPoints.size(),true);
+        width = low+(high-low)/2;
+        if (width == prevwidth || low>high) { //needed to reassure the same width is not repeated again
+            ResultVec = result; //return the keypoints from the previous iteration
+            break;
+        }
+        result.clear();
+
+        for (unsigned int i=0;i<keyPoints.size();++i){
+            if (Included[i]==true){
+                Included[i] = false;
+                result.push_back(i);
+                int minx = keyPoints[i].pt.x-width; int maxx = keyPoints[i].pt.x+width; //defining square boundaries around the point
+                int miny= keyPoints[i].pt.y-width; int maxy= keyPoints[i].pt.y+width;
+                if (minx<0) minx = 0; if (miny<0) miny = 0;
+
+                std::vector<u16 *> *he = treeANMS.search(minx, maxx, miny, maxy);
+                for (unsigned int j=0; j<he->size();j++) if (Included[(u64) (*he)[j]]) Included[(u64) (*he)[j]] = false;
+                delete he;
+                he = NULL;
+            }
+        }
+        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;
+    }
+    // retrieve final keypoints
+    vector<cv::KeyPoint> kp;
+    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
+
+    return kp;
+}
+
+vector<int> Ssc(const vector<cv::KeyPoint>& keyPoints, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows)
+{
+    assert(!keyPoints.empty() && "[vision_utils]: ANMS SSC failed. Input KeyPoints vector is empty.");
+
+    // several temp expression variables to simplify solution equation
+    int exp1 = rows + cols + 2*numRetPoints;
+    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
+    double exp3 = sqrt(exp2);
+    double exp4 = (2*(numRetPoints - 1));
+
+    double sol1 = -round((exp1+exp3)/exp4); // first solution
+    double sol2 = -round((exp1-exp3)/exp4); // second solution
+
+    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
+    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
+
+    int width;
+    int prevWidth = -1;
+
+    vector<int> ResultVec;
+    bool complete = false;
+    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
+
+    vector<int> result; result.reserve(keyPoints.size());
+    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 = width/2; //initializing Grid
+        int numCellCols = floor(cols/c);
+        int numCellRows = floor(rows/c);
+        vector<vector<bool> > coveredVec(numCellRows+1,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 w
+                    }
+                }
+            }
+        }
+
+        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 Ssc(const vector<cv::KeyPoint>& keyPoints, vector<cv::KeyPoint>& keyPointsSorted, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows)
+{
+    assert(!keyPoints.empty() && "[vision_utils]: ANMS SSC failed. Input KeyPoints vector is empty.");
+    assert(keyPointsSorted.empty() && "[vision_utils]: ANMS SSC failed. Output KeyPoints vector is not empty.");
+
+    vector<int> ResultVec = Ssc(keyPoints,numRetPoints,tolerance,cols,rows);
+
+    for (unsigned int i = 0; i<ResultVec.size(); i++)
+        keyPointsSorted.push_back(keyPoints[ResultVec[i]]);
+}
+
+void Ssc(const vector<cv::KeyPoint>& keyPoints, vector<cv::KeyPoint>& keyPointsSorted, const cv::Mat& desc, cv::Mat& descSorted, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows)
+{
+    assert(!keyPoints.empty() && "[vision_utils]: ANMS SSC failed. Input KeyPoints vector is empty.");
+    assert(keyPointsSorted.empty() && "[vision_utils]: ANMS SSC failed. Output KeyPoints vector is not empty.");
+
+    vector<int> ResultVec = Ssc(keyPoints,numRetPoints,tolerance,cols,rows);
+
+    assert(!desc.empty() && "[vision_utils]: sortByResponse failed. Matrix of input descriptors is empty.");
+    assert(keyPoints.size() == desc.rows && "[vision_utils]: sortByResponse failed. Input Keypoint and descriptor sizes must be equal.");
+    assert(descSorted.empty() && "[vision_utils]: sortByResponse failed. Output descriptors matrix is not empty.");
+
+    for (unsigned int i = 0; i<ResultVec.size(); i++)
+    {
+        keyPointsSorted.push_back(keyPoints[ResultVec[i]]);
+        descSorted.push_back(desc.row(ResultVec[i]));
+    }
+}
+
+
+void VisualizeAll(cv::Mat Image, vector<cv::KeyPoint> keyPoints, string figureTitle)
+{
+    cv::Mat resultImg;
+    cv::drawKeypoints(Image, keyPoints, resultImg, cv::Scalar(94.0, 206.0, 165.0, 0.0));
+    cv::namedWindow(figureTitle, cv::WINDOW_AUTOSIZE); cv::imshow(figureTitle, resultImg);
+    return;
+}
+
+
+} /* namespace vision_utils */
+

src/algorithms/anms/anms.h

@@ -7,16 +7,12 @@
 #include "nanoflann.hpp"
 #include "range-tree/ranget.h"
 
+namespace vision_utils {
+
 using namespace std;
 using namespace nanoflann;
 
-vector<cv::KeyPoint> TopN(vector<cv::KeyPoint> keyPoints, int numRetPoints)
-{
-    vector<cv::KeyPoint> kp;
-    for (int i = 0; i < numRetPoints; i++) kp.push_back(keyPoints[i]); //simply extracting numRetPoints keyPoints
-
-    return kp;
-}
+vector<cv::KeyPoint> TopN(vector<cv::KeyPoint> keyPoints, int numRetPoints);
 
 #if CV_MAJOR_VERSION < 3   // If you are using OpenCV 2
 vector<cv::KeyPoint> GridFAST(cv::Mat Image, int numRetPoints, int gridRows, int gridCols){
@@ -34,84 +30,10 @@ struct sort_pred {
     }
 };
 
-vector<cv::KeyPoint> BrownANMS(vector<cv::KeyPoint> keyPoints, int numRetPoints) {
-    vector<pair<float,int> > results;
-    results.push_back(make_pair(FLT_MAX,0));
-    for (unsigned int i = 1;i<keyPoints.size();++i){ //for every keyPoint we get the min distance to the previously visited keyPoints
-        float minDist = FLT_MAX;
-        for (unsigned int j=0;j<i;++j){
-            float exp1 = (keyPoints[j].pt.x-keyPoints[i].pt.x);
-            float exp2 = (keyPoints[j].pt.y-keyPoints[i].pt.y);
-            float curDist = sqrt(exp1*exp1+exp2*exp2);
-            minDist = min(curDist,minDist);
-        }
-        results.push_back(make_pair(minDist,i));
-    }
-    sort(results.begin(),results.end(),sort_pred()); //sorting by radius
-    vector<cv::KeyPoint> kp;
-    for (int i=0;i<numRetPoints;++i) kp.push_back(keyPoints[results[i].second]); //extracting numRetPoints keyPoints
+vector<cv::KeyPoint> BrownANMS(vector<cv::KeyPoint> keyPoints, int numRetPoints);
 
-    return kp;
-}
-
-
-
-vector<cv::KeyPoint> Sdc(vector<cv::KeyPoint> keyPoints, int numRetPoints, float tolerance, int cols, int rows){
-    double eps_var = 0.25; //this parameter is chosen to be the most optimal in the original paper
-
-    int low = 1; int high = cols; //binary search range initialization
-    int radius;
-    int prevradius = -1;
-
-    vector<int> ResultVec;
-    bool complete = false;
-    unsigned int K = numRetPoints;
-    unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
-
-    vector<int> result; result.reserve(keyPoints.size());
-    while(!complete){
-        radius = low+(high-low)/2;
-        if (radius == prevradius || 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 = eps_var*radius/sqrt(2); //initializing Grid
-        int numCellCols = floor(cols/c);
-        int numCellRows = floor(rows/c);
-        vector<vector<bool> > coveredVec(numCellRows+1,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(radius/c))>=0)? (row-floor(radius/c)) : 0; //get range which current radius is covering
-                int rowMax = ((row+floor(radius/c))<=numCellRows)? (row+floor(radius/c)) : numCellRows;
-                int colMin = ((col-floor(radius/c))>=0)? (col-floor(radius/c)) : 0;
-                int colMax = ((col+floor(radius/c))<=numCellCols)? (col+floor(radius/c)) : numCellCols;
-                for (int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov){
-                    for (int colToCov=colMin ; colToCov<=colMax; ++colToCov){
-                        double dist = sqrt((rowToCov-row)*(rowToCov-row) + (colToCov-col)*(colToCov-col));
-                        if (dist<=((double)radius)/c) coveredVec[rowToCov][colToCov] = true; //check the distance to every cell
-                    }
-                }
-            }
-        }
-        if (result.size()>=Kmin && result.size()<=Kmax){ //solution found
-            ResultVec = result;
-            complete = true;
-        }
-        else if (result.size()<Kmin) high = radius-1; //update binary search range
-        else low = radius+1;
-    }
-    // retrieve final keypoints
-    vector<cv::KeyPoint> kp;
-    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
-
-    return kp;
-}
 
+vector<cv::KeyPoint> Sdc(vector<cv::KeyPoint> keyPoints, int numRetPoints, float tolerance, int cols, int rows);
 
 /*kdtree algorithm*/
 template <typename T>
@@ -157,209 +79,16 @@ void generatePointCloud(PointCloud<T> &point, vector<cv::KeyPoint> keyPoints)
     }
 }
 
+vector<cv::KeyPoint> KdTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance,int cols,int rows);
 
-vector<cv::KeyPoint> KdTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance,int cols,int rows){
-	// several temp expression variables to simplify solution equation
-    int exp1 = rows + cols + 2*numRetPoints;
-    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
-    double exp3 = sqrt(exp2);
-    double exp4 = (2*(numRetPoints - 1));
-
-    double sol1 = -round((exp1+exp3)/exp4); // first solution
-    double sol2 = -round((exp1-exp3)/exp4); // second solution
-
-    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
-    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
-
+vector<cv::KeyPoint> RangeTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance, int cols, int rows);
 
-    PointCloud<int> cloud; //creating k-d tree with keypoints
-    generatePointCloud(cloud,keyPoints);
-    typedef KDTreeSingleIndexAdaptor< L2_Simple_Adaptor<int, PointCloud<int> > , PointCloud<int>, 2> my_kd_tree_t;
-    my_kd_tree_t   index(2, cloud, KDTreeSingleIndexAdaptorParams(25 /* max leaf */) );
-    index.buildIndex();
+vector<int> Ssc(const vector<cv::KeyPoint>& keyPoints, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows);
+void Ssc(const vector<cv::KeyPoint>& keyPoints, vector<cv::KeyPoint>& keyPointsSorted, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows);
+void Ssc(const vector<cv::KeyPoint>& keyPoints, vector<cv::KeyPoint>& keyPointsSorted, const cv::Mat& desc, cv::Mat& descSorted, const int& numRetPoints, const float& tolerance, const int& cols, const int& rows);
 
-    bool complete = false;
-    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
-    vector<int> ResultVec;
-    int radius; int prevradius = -1;
-
-    vector<int> result; result.reserve(keyPoints.size());
-    while(!complete){
-        vector<bool> Included(keyPoints.size(),true);
-        radius = low+(high-low)/2;
-        if (radius == prevradius || low>high) { //needed to reassure the same radius is not repeated again
-            ResultVec = result; //return the keypoints from the previous iteration
-            break;
-        }
-        result.clear();
-
-        for (unsigned int i=0;i<keyPoints.size();++i){
-            if (Included[i]==true){
-                Included[i] = false;
-                result.push_back(i);
-                const int search_radius = static_cast<int>(radius*radius);
-                vector<pair<size_t,int> >   ret_matches;
-                nanoflann::SearchParams params;
-                const int query_pt[2] = { (int)keyPoints[i].pt.x, (int)keyPoints[i].pt.y};
-                const size_t nMatches = index.radiusSearch(&query_pt[0],search_radius, ret_matches, params);
-
-                for (size_t nmIdx=0; nmIdx<nMatches; nmIdx++) {
-                    if (Included[ret_matches[nmIdx].first]) Included[ret_matches[nmIdx].first] = false;
-                }
-            }
-        }
-
-        if (result.size()>=Kmin && result.size()<=Kmax){ //solution found
-            ResultVec = result;
-            complete = true;
-        }
-        else if (result.size()<Kmin) high = radius-1; //update binary search range
-        else low = radius+1;
-
-        prevradius = radius;
-    }
-
-    // retrieve final keypoints
-    vector<cv::KeyPoint> kp;
-    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
+void VisualizeAll(cv::Mat Image, vector<cv::KeyPoint> keyPoints, string figureTitle);
 
-    return kp;
-}
-
-vector<cv::KeyPoint> RangeTree(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance, int cols, int rows)
-{
-	// several temp expression variables to simplify solution equation
-    int exp1 = rows + cols + 2*numRetPoints;
-    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
-    double exp3 = sqrt(exp2);
-    double exp4 = (2*(numRetPoints - 1));
-
-    double sol1 = -round((exp1+exp3)/exp4); // first solution
-    double sol2 = -round((exp1-exp3)/exp4); // second solution
-
-    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
-    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
-
-    rangetree<u16, u16> treeANMS(keyPoints.size(), keyPoints.size()); //creating range tree with keypoints
-    for (unsigned int i = 0; i < keyPoints.size(); i++) treeANMS.add(keyPoints[i].pt.x, keyPoints[i].pt.y, (u16 *)(intptr_t)i);
-    treeANMS.finalize();
-
-    bool complete = false;
-    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
-    vector<int> ResultVec;
-    int width;
-    int prevwidth = -1;
-
-    vector<int> result; result.reserve(keyPoints.size());
-    while(!complete){
-        vector<bool> Included(keyPoints.size(),true);
-        width = low+(high-low)/2;
-        if (width == prevwidth || low>high) { //needed to reassure the same width is not repeated again
-            ResultVec = result; //return the keypoints from the previous iteration
-            break;
-        }
-        result.clear();
-
-        for (unsigned int i=0;i<keyPoints.size();++i){
-            if (Included[i]==true){
-                Included[i] = false;
-                result.push_back(i);
-                int minx = keyPoints[i].pt.x-width; int maxx = keyPoints[i].pt.x+width; //defining square boundaries around the point
-                int miny= keyPoints[i].pt.y-width; int maxy= keyPoints[i].pt.y+width;
-                if (minx<0) minx = 0; if (miny<0) miny = 0;
-
-                std::vector<u16 *> *he = treeANMS.search(minx, maxx, miny, maxy);
-                for (unsigned int j=0; j<he->size();j++) if (Included[(u64) (*he)[j]]) Included[(u64) (*he)[j]] = false;
-                delete he;
-                he = NULL;
-            }
-        }
-        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;
-    }
-    // retrieve final keypoints
-    vector<cv::KeyPoint> kp;
-    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
-
-    return kp;
-}
-
-vector<cv::KeyPoint> Ssc(vector<cv::KeyPoint> keyPoints, int numRetPoints,float tolerance, int cols, int rows){
-    // several temp expression variables to simplify solution equation
-    int exp1 = rows + cols + 2*numRetPoints;
-    long long exp2 = ((long long) 4*cols + (long long)4*numRetPoints + (long long)4*rows*numRetPoints + (long long)rows*rows + (long long) cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*numRetPoints);
-    double exp3 = sqrt(exp2);
-    double exp4 = (2*(numRetPoints - 1));
-
-    double sol1 = -round((exp1+exp3)/exp4); // first solution
-    double sol2 = -round((exp1-exp3)/exp4); // second solution
-
-    int high = (sol1>sol2)? sol1 : sol2; //binary search range initialization with positive solution
-    int low = floor(sqrt((double)keyPoints.size()/numRetPoints));
-
-    int width;
-    int prevWidth = -1;
-
-    vector<int> ResultVec;
-    bool complete = false;
-    unsigned int K = numRetPoints; unsigned int Kmin = round(K-(K*tolerance)); unsigned int Kmax = round(K+(K*tolerance));
-
-    vector<int> result; result.reserve(keyPoints.size());
-    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 = width/2; //initializing Grid
-        int numCellCols = floor(cols/c);
-        int numCellRows = floor(rows/c);
-        vector<vector<bool> > coveredVec(numCellRows+1,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 w
-                    }
-                }
-            }
-        }
-
-        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;
-    }
-    // retrieve final keypoints
-    vector<cv::KeyPoint> kp;
-    for (unsigned int i = 0; i<ResultVec.size(); i++) kp.push_back(keyPoints[ResultVec[i]]);
-
-    return kp;
-}
-
-
-void VisualizeAll(cv::Mat Image, vector<cv::KeyPoint> keyPoints, string figureTitle){
-    cv::Mat resultImg;
-    cv::drawKeypoints(Image, keyPoints, resultImg, cv::Scalar(94.0, 206.0, 165.0, 0.0));
-    cv::namedWindow(figureTitle, cv::WINDOW_AUTOSIZE); cv::imshow(figureTitle, resultImg);
-    return;
-}
+} /* namespace vision_utils */
 
 #endif // ANMS_H

src/examples/test_algorithm_anms.cpp

@@ -130,16 +130,19 @@ int main(void)
     	cv::imshow("ORIGINAL", resultImg);
     	cv::waitKey(1);
 
-    	//Sorting keypoints by deacreasing order of strength
-    	vector<int> responseVector;
-    	for (unsigned int i =0 ; i<keyPoints.size(); i++)
-    		responseVector.push_back(keyPoints[i].response);
-    	vector<int> Indx(responseVector.size());
-    	std::iota (std::begin(Indx), std::end(Indx), 0);
-    	cv::sortIdx(responseVector, Indx, CV_SORT_DESCENDING);
-    	vector<cv::KeyPoint> keyPointsSorted;
-    	for (unsigned int i = 0; i < keyPoints.size(); i++)
-    		keyPointsSorted.push_back(keyPoints[Indx[i]]);
+    	std::vector<cv::KeyPoint> keyPointsSorted;
+    	vision_utils::sortByResponse(keyPoints, keyPointsSorted);
+
+//    	//Sorting keypoints by deacreasing order of strength
+//    	vector<int> responseVector;
+//    	for (unsigned int i =0 ; i<keyPoints.size(); i++)
+//    		responseVector.push_back(keyPoints[i].response);
+//    	vector<int> Indx(responseVector.size());
+//    	std::iota (std::begin(Indx), std::end(Indx), 0);
+//    	cv::sortIdx(responseVector, Indx, CV_SORT_DESCENDING);
+//    	vector<cv::KeyPoint> keyPointsSorted;
+//    	for (unsigned int i = 0; i < keyPoints.size(); i++)
+//    		keyPointsSorted.push_back(keyPoints[Indx[i]]);
 
     	//int numRetPoints = 10; //choose exact number of return points
     	float percentage = 0.2; //or choose percentage of points to be return
@@ -148,7 +151,9 @@ int main(void)
     	float tolerance = 0.00001; // tolerance of the number of return points
 
     	// SSC ANMS
-    	vector<cv::KeyPoint> sscKP = Ssc(keyPointsSorted,numRetPoints,tolerance,sen_ptr->getImage().cols,sen_ptr->getImage().rows);
+    	vector<cv::KeyPoint> sscKP;
+    	vision_utils::Ssc(keyPointsSorted, sscKP, numRetPoints, tolerance, sen_ptr->getImage().cols, sen_ptr->getImage().rows);
+
 
     	cv::drawKeypoints(sen_ptr->getImage(), sscKP, resultImg, cv::Scalar(94.0, 206.0, 165.0, 0.0));
     	cv::imshow("ANMS TEST", resultImg);

src/vision_utils.cpp

@@ -8,6 +8,49 @@
 namespace vision_utils
 {
 
+std::vector<int> sortByResponse(const KeyPointVector& _kps_in, const int& _type)
+{
+    assert(!_kps_in.empty() && "[vision_utils]: sortByResponse failed. Input KeyPoints vector is empty.");
+
+    //Sorting keypoints by deacreasing order of strength
+    std::vector<int> responseVector;
+    for (unsigned int ii =0 ; ii<_kps_in.size(); ii++)
+        responseVector.push_back(_kps_in[ii].response);
+    std::vector<int> Indx(responseVector.size());
+    std::iota (std::begin(Indx), std::end(Indx), 0);
+    cv::sortIdx(responseVector, Indx, _type);
+
+    return Indx;
+}
+
+void sortByResponse(const KeyPointVector& _kps_in, KeyPointVector& _kps_out, const int& _type)
+{
+    assert(!_kps_in.empty() && "[vision_utils]: sortByResponse failed. Input KeyPoints vector is empty.");
+    assert(_kps_out.empty() && "[vision_utils]: sortByResponse failed. Output KeyPoints vector is not empty.");
+
+    std::vector<int> Indx = sortByResponse(_kps_in, _type);
+    for (unsigned int ii = 0; ii < _kps_in.size(); ii++)
+        _kps_out.push_back(_kps_in[Indx[ii]]);
+}
+
+void sortByResponse(const KeyPointVector& _kps_in, KeyPointVector& _kps_out, const cv::Mat& _des_in, cv::Mat& _des_out, const int& _type)
+{
+    assert(!_kps_in.empty() && "[vision_utils]: sortByResponse failed. Input KeyPoints vector is empty.");
+    assert(_kps_out.empty() && "[vision_utils]: sortByResponse failed. Output KeyPoints vector is not empty.");
+
+    std::vector<int> Indx = sortByResponse(_kps_in, _type);
+
+    assert(!_des_in.empty() && "[vision_utils]: sortByResponse failed. Matrix of input descriptors is empty.");
+    assert(_kps_in.size() == _des_in.rows && "[vision_utils]: sortByResponse failed. Input Keypoint and descriptor sizes must be equal.");
+    assert(_des_out.empty() && "[vision_utils]: sortByResponse failed. Output descriptors matrix is not empty.");
+
+    for (unsigned int ii = 0; ii < _kps_in.size(); ii++)
+    {
+        _kps_out.push_back(_kps_in[Indx[ii]]);
+        _des_out.push_back(_des_in.row(Indx[ii]));
+    }
+}
+
 bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs)
 {
     return (lhs.x < rhs.x) || ((lhs.x == rhs.x) && (lhs.y < rhs.y));

src/vision_utils.h

@@ -314,6 +314,12 @@ class FeatureIdxGrid {
         Grid grid_;
 };
 
+static const cv::Mat const_mat = cv::Mat();
+
+std::vector<int> sortByResponse(const KeyPointVector& _kps_in, const int& _type = CV_SORT_DESCENDING);
+void sortByResponse(const KeyPointVector& _kps_in, KeyPointVector& _kps_out, const int& _type = CV_SORT_DESCENDING);
+void sortByResponse(const KeyPointVector& _kps_in, KeyPointVector& _kps_out, const cv::Mat& _des_in, cv::Mat& _des_out, const int& _type = CV_SORT_DESCENDING);
+
 bool LessPoints(const cv::Point2f& lhs, const cv::Point2f& rhs);
 bool LessKPoints(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs);