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

Add ANMS (SSC method) (#1276)

Browse Source
This commit is contained in:
Borong Yuan
2024-05-19 18:21:00 -07:00
committed by GitHub
parent 0d4e4730c7
commit fbeabf0751
12 changed files with 321 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Features2d.h
+7 -5
View File
@@ -192,16 +192,17 @@ public:
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_;}
@@ -234,6 +235,7 @@ private:
private:
ParametersMap parameters_;
int maxFeatures_;
bool SSC_;
float _maxDepth; // 0=inf
float _minDepth;
std::vector<float> _roiRatios; // size 4
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -334,6 +334,7 @@ private:
bool _rotateImagesUpsideUp;
bool _createOccupancyGrid;
int _visMaxFeatures;
bool _visSSC;
bool _imagesAlreadyRectified;
bool _rectifyOnlyFeatures;
bool _covOffDiagonalIgnored;
corelib/include/rtabmap/core/Parameters.h
+2
View File
@@ -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)
@@ -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.");
corelib/include/rtabmap/core/util2d.h
+3
View File
@@ -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.
*
corelib/src/Features2d.cpp
+118 -60
View File
@@ -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());
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
if(keypoints3D.size())
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[k] = keypoints3D[iter->second];
kpts3DTmp.resize(ResultVec.size());
}
if(descriptors.rows)
{
if(descriptors.type() == CV_32FC1)
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())
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
kpts3DTmp[k] = keypoints3D[ResultVec[k]];
}
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>(ResultVec[k]), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(ResultVec[k]), 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);
else
{
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
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)
{
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())
{
kpts3DTmp[k] = keypoints3D[iter->second];
}
if(descriptors.rows)
{
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(), !ssc&&kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
keypoints3D = kpts3DTmp;
@@ -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)
{
//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
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)
int removed;
inliers.resize(keypoints.size(), false);
if(ssc)
{
inliers[iter->second] = true;
minimumHessian = iter->first;
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;
}
}
else
{
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());
@@ -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)
{
@@ -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])
@@ -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)),
@@ -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);
@@ -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)
{
@@ -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!");
corelib/src/Memory.cpp
+31 -18
View File
@@ -111,6 +111,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visSSC(Parameters::defaultVisSSC()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
@@ -603,6 +604,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kVisSSC(), _visSSC);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -5131,9 +5133,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if((int)keypoints.size() > maxFeatures)
{
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
else
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -5349,22 +5355,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
int inliersCount = 0;
if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
{
Feature2D::limitKeypoints(keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols());
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
{
++inliersCount;
}
}
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
}
else
{
@@ -5373,8 +5370,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
}
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
inliersCount = _feature2D->getMaxFeatures();
if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getSSC());
}
else
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
}
}
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
++inliersCount;
}
descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
corelib/src/RegistrationVis.cpp
+5
View File
@@ -102,6 +102,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), _featureParameters.at(Parameters::kVisSSC())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -234,6 +235,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
}
if(uContains(parameters, Parameters::kVisSSC()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
}
if(uContains(parameters, Parameters::kVisMaxDepth()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
corelib/src/SensorCaptureThread.cpp
+1
View File
@@ -241,6 +241,7 @@ void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameter
ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
uInsert(params, ParametersPair(Parameters::kKpSSC(), uValue(params, Parameters::kVisSSC(), defaultParams.at(Parameters::kVisSSC()))));
uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
corelib/src/util2d.cpp
+77
View File
@@ -2202,6 +2202,83 @@ void NMS(
}
}
std::vector<int> SSC(
const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
{
// 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);
double exp3 = sqrt(exp2);
double exp4 = maxKeypoints - 1;
double sol1 = -round((exp1 + exp3) / exp4); // first solution
double sol2 = -round((exp1 - exp3) / exp4); // second solution
// binary search range initialization with positive solution
int high = (sol1 > sol2) ? sol1 : sol2;
int low = floor(sqrt((double)keypoints.size() / maxKeypoints));
low = std::max(1, low);
int width;
int prevWidth = -1;
unsigned int Kmin = round(maxKeypoints - (maxKeypoints * tolerance));
unsigned int Kmax = round(maxKeypoints + (maxKeypoints * tolerance));
std::vector<int> ResultVec, result;
result.reserve(keypoints.size());
bool complete = false;
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 = (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));
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
}
}
}
}
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 rotateImagesUpsideUpIfNecessary(
CameraModel & model,
cv::Mat & rgb,
guilib/src/DatabaseViewer.cpp
+1
View File
@@ -5732,6 +5732,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
// generate kpts
std::vector<cv::KeyPoint> kpts;
uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(parameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
guilib/src/PreferencesDialog.cpp
+5 -2
View File
@@ -1029,6 +1029,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_memDepthAsMask->setObjectName(Parameters::kMemDepthAsMask().c_str());
_ui->checkBox_memStereoFromMotion->setObjectName(Parameters::kMemStereoFromMotion().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
_ui->checkBox_kp_ssc->setObjectName(Parameters::kKpSSC().c_str());
_ui->spinBox_KPGridRows->setObjectName(Parameters::kKpGridRows().c_str());
_ui->spinBox_KPGridCols->setObjectName(Parameters::kKpGridCols().c_str());
_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
@@ -1245,9 +1246,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility()));
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
_ui->checkBox__visCorGuessMatchToProjection->setObjectName(Parameters::kVisCorGuessMatchToProjection().c_str());
_ui->checkBox_visCorGuessMatchToProjection->setObjectName(Parameters::kVisCorGuessMatchToProjection().c_str());
_ui->vis_feature_detector->setObjectName(Parameters::kVisFeatureType().c_str());
_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
_ui->checkBox_visSSC->setObjectName(Parameters::kVisSSC().c_str());
_ui->reextract_gridrows->setObjectName(Parameters::kVisGridRows().c_str());
_ui->reextract_gridcols->setObjectName(Parameters::kVisGridCols().c_str());
_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
@@ -4180,7 +4182,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
{
Src previousCameraSrc = getSourceDriver();
Src previousLidarSrc = getLidarSourceDriver();
if(_ui->comboBox_imuFilter_strategy->currentIndex()==0)
{
_ui->comboBox_imuFilter_strategy->setCurrentIndex(2);
@@ -5376,6 +5378,7 @@ void PreferencesDialog::useOdomFeatures()
_ui->surf_doubleSpinBox_maxDepth->setValue(_ui->loopClosure_bowMaxDepth->value());
_ui->surf_doubleSpinBox_minDepth->setValue(_ui->loopClosure_bowMinDepth->value());
_ui->surf_spinBox_wordsPerImageTarget->setValue(_ui->reextract_maxFeatures->value());
_ui->checkBox_kp_ssc->setChecked(_ui->checkBox_visSSC->isChecked());
_ui->spinBox_KPGridRows->setValue(_ui->reextract_gridrows->value());
_ui->spinBox_KPGridCols->setValue(_ui->reextract_gridcols->value());
_ui->lineEdit_kp_roi->setText(_ui->loopClosure_roi->text());
guilib/src/ui/preferencesDialog.ui
+70 -72
View File
@@ -10587,14 +10587,8 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_81">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).</string>
</property>
@@ -10606,7 +10600,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_99">
<property name="text">
<string>Top ROI ratio (0 = no change).</string>
@@ -10619,7 +10613,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ratioBadSign">
<property name="decimals">
<number>2</number>
@@ -10640,12 +10634,6 @@ generate the number of words requested.</string>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_57">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Minimum words depth. Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
</property>
@@ -10741,14 +10729,14 @@ generate the number of words requested.</string>
</item>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QLineEdit" name="lineEdit_kp_roi">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_101">
<property name="text">
<string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
@@ -10763,12 +10751,6 @@ generate the number of words requested.</string>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_53">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Maximum words depth (0 means inf). Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
</property>
@@ -10780,7 +10762,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
<property name="suffix">
<string> %</string>
@@ -10834,11 +10816,31 @@ generate the number of words requested.</string>
<number>2000</number>
</property>
<property name="value">
<number>150</number>
<number>500</number>
</property>
</widget>
</item>
<item row="8" column="0">
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_kp_ssc">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_755">
<property name="text">
<string>If true, SSC (Suppression via Square Covering) is applied to limit keypoints.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
<property name="suffix">
<string> %</string>
@@ -10848,7 +10850,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_97">
<property name="text">
<string>Left ROI ratio (0 = no change).</string>
@@ -10861,7 +10863,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_98">
<property name="text">
<string>Right ROI ratio (0 = no change).</string>
@@ -10874,7 +10876,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi2">
<property name="suffix">
<string> %</string>
@@ -10884,7 +10886,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="11" column="0">
<item row="12" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi3">
<property name="suffix">
<string> %</string>
@@ -10894,7 +10896,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_100">
<property name="text">
<string>Bottom ROI ratio (0 = no change).</string>
@@ -10929,14 +10931,8 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QLabel" name="label_50">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Number of rows of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
</property>
@@ -10948,7 +10944,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QSpinBox" name="spinBox_KPGridRows">
<property name="minimum">
<number>1</number>
@@ -10961,7 +10957,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="13" column="0">
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_KPGridCols">
<property name="minimum">
<number>1</number>
@@ -10974,14 +10970,8 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="13" column="1">
<item row="14" column="1">
<widget class="QLabel" name="label_86">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Number of columns of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
</property>
@@ -10995,12 +10985,6 @@ generate the number of words requested.</string>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_262">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Use depth image as mask when extracting features.</string>
</property>
@@ -11021,12 +11005,6 @@ generate the number of words requested.</string>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_582">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Triangulate features without depth using stereo from motion (odometry). It would be ignored if depth as mask is checked and the feature detector used supports masking.</string>
</property>
@@ -21684,7 +21662,7 @@ Lower the ratio -&gt; higher the precision.</string>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox__visCorGuessMatchToProjection">
<widget class="QCheckBox" name="checkBox_visCorGuessMatchToProjection">
<property name="text">
<string/>
</property>
@@ -22461,7 +22439,7 @@ Lower the ratio -&gt; higher the precision.</string>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_25" columnstretch="0,1">
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_237">
<property name="text">
<string>Maximum feature depth.</string>
@@ -22474,7 +22452,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_261">
<property name="text">
<string>ROI ratios [left right top bottom] between 0 and 1.</string>
@@ -22487,7 +22465,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QLineEdit" name="loopClosure_roi"/>
</item>
<item row="2" column="0">
@@ -22510,7 +22488,27 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_visSSC">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_756">
<property name="text">
<string>If true, SSC (Suppression via Square Covering) is applied to limit keypoints.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_275">
<property name="text">
<string>Minimum feature depth.</string>
@@ -22613,7 +22611,7 @@ Lower the ratio -&gt; higher the precision.</string>
</item>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_bowMaxDepth">
<property name="suffix">
<string> m</string>
@@ -22636,7 +22634,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_bowMinDepth">
<property name="suffix">
<string> m</string>
@@ -22649,7 +22647,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QSpinBox" name="reextract_gridrows">
<property name="minimum">
<number>1</number>
@@ -22662,7 +22660,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_438">
<property name="text">
<string>Number of rows of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
@@ -22675,7 +22673,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_439">
<property name="text">
<string>Number of columns of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
@@ -22688,7 +22686,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QSpinBox" name="reextract_gridcols">
<property name="minimum">
<number>1</number>
@@ -22701,7 +22699,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_450">
<property name="text">
<string>Use depth image as mask when extracting features.</string>
@@ -22714,7 +22712,7 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_visDepthAsMask">
<property name="text">
<string/>