Skip to content

Commit

Permalink
Add ANMS (SSC method) (#1276)
Browse files Browse the repository at this point in the history
  • Loading branch information
@borongyuan
borongyuan committed May 20, 2024
1 parent 0d4e473 commit fbeabf0
Show file tree
Hide file tree
Showing 12 changed files with 318 additions and 154 deletions.
12 changes: 7 additions & 5 deletions corelib/include/rtabmap/core/Features2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,16 +192,17 @@ class RTABMAP_CORE_EXPORT Feature2D {
const cv::Mat & disparity,
float minDisparity);

static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);

static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

int getMaxFeatures() const {return maxFeatures_;}
bool getSSC() const {return SSC_;}
float getMinDepth() const {return _minDepth;}
float getMaxDepth() const {return _maxDepth;}
int getGridRows() const {return gridRows_;}
Expand Down Expand Up @@ -234,6 +235,7 @@ class RTABMAP_CORE_EXPORT Feature2D {
private:
ParametersMap parameters_;
int maxFeatures_;
bool SSC_;
float _maxDepth; // 0=inf
float _minDepth;
std::vector<float> _roiRatios; // size 4
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/Memory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,6 +334,7 @@ class RTABMAP_CORE_EXPORT Memory
bool _rotateImagesUpsideUp;
bool _createOccupancyGrid;
int _visMaxFeatures;
bool _visSSC;
bool _imagesAlreadyRectified;
bool _rectifyOnlyFeatures;
bool _covOffDiagonalIgnored;
Expand Down
2 changes: 2 additions & 0 deletions corelib/include/rtabmap/core/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,6 +244,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Kp, MaxDepth, float, 0, "Filter extracted keypoints by depth (0=inf).");
RTABMAP_PARAM(Kp, MinDepth, float, 0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, MaxFeatures, int, 500, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
Expand Down Expand Up @@ -693,6 +694,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
#endif
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit).");
RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit).");
RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features.");
Expand Down
3 changes: 3 additions & 0 deletions corelib/include/rtabmap/core/util2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,9 @@ void RTABMAP_CORE_EXPORT NMS(
cv::Mat & descriptorsOut,
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);

/**
* @brief Rotate images and camera model so that the top of the image is up.
*
Expand Down
172 changes: 115 additions & 57 deletions corelib/src/Features2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,70 +268,111 @@ void Feature2D::filterKeypointsByDisparity(
}
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
cv::Mat descriptors;
limitKeypoints(keypoints, descriptors, maxKeypoints);
limitKeypoints(keypoints, descriptors, maxKeypoints, imageSize, ssc);
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
std::vector<cv::Point3f> keypoints3D;
limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints, imageSize, ssc);
}

void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold

// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}

// Remove them from the signature
int removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
int removed;
std::vector<cv::KeyPoint> kptsTmp;
std::vector<cv::Point3f> kpts3DTmp;
if(!keypoints3D.empty())
{
kpts3DTmp.resize(maxKeypoints);
}
cv::Mat descriptorsTmp;
if(descriptors.rows)
if(ssc)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
static constexpr float tolerance = 0.1;
auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
removed = keypoints.size()-ResultVec.size();
// retrieve final keypoints
kptsTmp.resize(ResultVec.size());
if(!keypoints3D.empty())
{
kpts3DTmp.resize(ResultVec.size());
}
if(descriptors.rows)
{
descriptorsTmp = cv::Mat(ResultVec.size(), descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k<ResultVec.size(); ++k)
{
kptsTmp[k] = keypoints[ResultVec[k]];
if(keypoints3D.size())
{
kpts3DTmp[k] = keypoints3D[ResultVec[k]];
}
if(descriptors.rows)
{
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(ResultVec[k]), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(ResultVec[k]), descriptors.cols*sizeof(char));
}
}
}
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
else
{
kptsTmp[k] = keypoints[iter->second];
if(keypoints3D.size())
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold

// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
kpts3DTmp[k] = keypoints3D[iter->second];
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}

// Remove them from the signature
removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
kptsTmp.resize(maxKeypoints);
if(!keypoints3D.empty())
{
kpts3DTmp.resize(maxKeypoints);
}
if(descriptors.rows)
{
if(descriptors.type() == CV_32FC1)
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k<kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
if(keypoints3D.size())
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
kpts3DTmp[k] = keypoints3D[iter->second];
}
else
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
}
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), !ssc&&kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
keypoints3D = kpts3DTmp;
Expand All @@ -342,31 +383,46 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
}
}

void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
// Remove words under the new hessian threshold

// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
float minimumHessian = 0.0f;
int removed;
inliers.resize(keypoints.size(), false);
if(ssc)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
static constexpr float tolerance = 0.1;
auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
removed = keypoints.size()-ResultVec.size();
for(unsigned int k=0; k<ResultVec.size(); ++k)
{
inliers[ResultVec[k]] = true;
}
}

// Keep keypoints with highest response
int removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
inliers.resize(keypoints.size(), false);
float minimumHessian = 0.0f;
for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
else
{
inliers[iter->second] = true;
minimumHessian = iter->first;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold

// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i<keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}

// Keep keypoints with highest response
removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
for(int k=0; k<maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
{
inliers[iter->second] = true;
minimumHessian = iter->first;
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
Expand All @@ -378,7 +434,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
}
}

void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc)
{
if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
{
Expand Down Expand Up @@ -406,7 +462,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
for(size_t i=0; i<keypointsPerCell.size(); ++i)
{
std::vector<bool> inliersCell;
limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell, cv::Size(colSize, rowSize), ssc);
for(size_t j=0; j<inliersCell.size(); ++j)
{
if(inliersCell[j])
Expand All @@ -432,6 +488,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
maxFeatures_(Parameters::defaultKpMaxFeatures()),
SSC_(Parameters::defaultKpSSC()),
_maxDepth(Parameters::defaultKpMaxDepth()),
_minDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
Expand All @@ -453,6 +510,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
uInsert(parameters_, parameters);

Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpSSC(), SSC_);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Expand Down Expand Up @@ -736,7 +794,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
subKeypoints = this->generateKeypointsImpl(image, roi, mask);
if (this->getType() != Feature2D::Type::kFeaturePyDetector)
{
limitKeypoints(subKeypoints, maxFeatures);
limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
}
if(roi.x || roi.y)
{
Expand Down Expand Up @@ -2142,7 +2200,7 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image

if((int)keypoints.size() > this->getMaxFeatures())
{
limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), roi.size(), this->getSSC());
}
#else
UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
Expand Down
Loading

0 comments on commit fbeabf0

Please sign in to comment.