From 2454b04f1f9ee3ec39a5b16728ce5aa48f458447 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sun, 27 Oct 2024 05:44:22 +0800 Subject: [PATCH] Sort keypoints before SSC (#1364) * Sort keypoints before SSC * avoid using vector --- corelib/include/rtabmap/core/Features2d.h | 1 + corelib/include/rtabmap/core/util2d.h | 2 +- corelib/src/Features2d.cpp | 36 +++++++++++++++++++++-- corelib/src/util2d.cpp | 23 ++++++--------- 4 files changed, 45 insertions(+), 17 deletions(-) diff --git a/corelib/include/rtabmap/core/Features2d.h b/corelib/include/rtabmap/core/Features2d.h index a08b1e7a7f..f26f84be73 100644 --- a/corelib/include/rtabmap/core/Features2d.h +++ b/corelib/include/rtabmap/core/Features2d.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "rtabmap/core/Parameters.h" #include "rtabmap/core/SensorData.h" diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index edec1ed9b3..e0b5426889 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/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 RTABMAP_CORE_EXPORT SSC( - const std::vector & keypoints, int maxKeypoints, float tolerance, int cols, int rows); + const std::vector & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector & indx = {}); /** * @brief Rotate images and camera model so that the top of the image is up. diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 80a2ece26c..f350462991 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -301,8 +301,24 @@ void Feature2D::limitKeypoints(std::vector & 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 responseVector; + for (unsigned int i = 0; i < keypoints.size(); i++) + { + responseVector.push_back(keypoints[i].response); + } + std::vector 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 & 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 responseVector; + for (unsigned int i = 0; i < keypoints.size(); i++) + { + responseVector.push_back(keypoints[i].response); + } + std::vector 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 SSC( - const std::vector & keypoints, int maxKeypoints, float tolerance, int cols, int rows) + const std::vector & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector & 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 SSC( double c = (double)width / 2.0; // initializing Grid int numCellCols = floor(cols / c); int numCellRows = floor(rows / c); - std::vector> coveredVec(numCellRows+1, std::vector(numCellCols+1, false)); + cv::Mat coveredMask = cv::Mat::zeros(numCellRows + 1, numCellCols + 1, CV_8UC1); for(unsigned int i=0; i(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) - { - for(int colToCov=colMin; colToCov<=colMax; ++colToCov) - { - if(!coveredVec[rowToCov][colToCov]) - coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width - } - } + coveredMask(cv::Range(rowMin, rowMax + 1), cv::Range(colMin, colMax + 1)) = 255; // cover cells within the square bounding box with width } }