i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sort keypoints before SSC (#1364)

Browse Source 
* Sort keypoints before SSC

* avoid using vector<bool>
This commit is contained in:
Borong Yuan
2024-10-27 05:44:22 +08:00
committed by GitHub
parent e3fde4ecdd
commit 2454b04f1f
4 changed files with 45 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Features2d.h
View File

@@ -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"

2
corelib/include/rtabmap/core/util2d.h
View File

@@ -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.

36
corelib/src/Features2d.cpp
View File

@@ -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)
{

23
corelib/src/util2d.cpp
View File

@@ -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 col = floor(keypoints[i].pt.x / c);
if(coveredVec[row][col] == false) // if the cell is not covered
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[useIndx?indx[i]:i].pt.x / c);
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)
{
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
}
}