diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 612f6b1c4158831da4779abbf27d8d95cb54c987..d00212ed8c5e481ece97c871e97d2f202aa1d4f5 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/algorithms/anms/anms.cpp b/src/algorithms/anms/anms.cpp new file mode 100644 index 0000000000000000000000000000000000000000..25104f8fc20ebe3a78d89015c631bb02dacba22e --- /dev/null +++ b/src/algorithms/anms/anms.cpp @@ -0,0 +1,327 @@ +#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 */ + diff --git a/src/algorithms/anms/anms.h b/src/algorithms/anms/anms.h index 9ce79ebb00da009a7773aefdbc6e15a6dee82de2..b1cba9382fb1cfe7c219204cdb12c2a26f1f5d71 100644 --- a/src/algorithms/anms/anms.h +++ b/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 diff --git a/src/examples/test_algorithm_anms.cpp b/src/examples/test_algorithm_anms.cpp index 8a7d146a335a996a71d7816c0029aba86940d41e..8fbb8a5ecb82a384d60e7e3725c2445c6ddf1764 100644 --- a/src/examples/test_algorithm_anms.cpp +++ b/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); diff --git a/src/vision_utils.cpp b/src/vision_utils.cpp index 3c3b95359fc7b563a707e30ec3400c1e89fa8440..a08505b5265eb8559db19c1a5564b1f2ba3c7d23 100644 --- a/src/vision_utils.cpp +++ b/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)); diff --git a/src/vision_utils.h b/src/vision_utils.h index 6959f25592aeb9700f793cfe20bff7e8333b38e4..d198fb97f2753a2d21b0bcd38db247b4fd010ad0 100644 --- a/src/vision_utils.h +++ b/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);