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

Cudasift tuning and SSC supporting multicameras (#1677)

Browse Source 
* CudaSIFT: filter doubles

* removed fixed threshold

* SSC can be used with multicameras. Refactored CudaSIFT to support SSC. Add new parameter SIFT/MaxGaussianThreshold. DbViewer: show negative features with gray color (so that we can know which features are in the vocabulary)

* Added SIFT/MaxGaussianThreshold parameter

* Updated parameter description
This commit is contained in:
matlabbe
2026-03-28 18:47:46 -07:00
committed by GitHub
parent 94bd3601fc
commit 20409d2bf6
11 changed files with 232 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
corelib/src/Features2d.cpp
View File

@@ -466,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
minimumHessian = iter->first;
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
}
else
@@ -1251,7 +1251,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
rootSIFT_(Parameters::defaultSIFTRootSIFT()),
gpu_(Parameters::defaultSIFTGpu()),
guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
upscale_(Parameters::defaultSIFTUpscale()),
cudaSiftData_(0),
cudaSiftMemory_(0),
@@ -1284,23 +1285,25 @@ void SIFT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
if(gpu_)
{
#ifdef RTABMAP_CUDASIFT
// Check if there is a cuda device
if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
UDEBUG("Init SiftData");
if(cudaSiftData_ == 0) {
if(cudaSiftData_==0)
{
if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
UDEBUG("Init SiftData");
cudaSiftData_ = new SiftData();
InitSiftData(*cudaSiftData_, 8192, true, true);
}
}
else{
UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
gpu_ = false;
else{
UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
gpu_ = false;
}
}
#else
UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1363,7 +1366,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
numOctaves = 7; // hard-coded limit in CudaSift
}
float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
float thresh = guaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */
float thresh = gaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */
float edgeLimit = edgeThreshold_;
float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
@@ -1388,15 +1391,9 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
cudaSiftDescriptors_ = cv::Mat();
if(cudaSiftData_->numPts)
{
int maxKeypoints = this->getMaxFeatures();
if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
{
maxKeypoints = cudaSiftData_->numPts;
}
// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
keypoints.resize(cudaSiftData_->numPts);
cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
size_t k=0;
for(int i=0; i<cudaSiftData_->numPts; ++i)
{
// Ignore keypoints with invalid descriptors
@@ -1413,29 +1410,40 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
continue;
}
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(abs(cudaSiftData_->h_data[i].sharpness), i));
}
if(i>0 &&
cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
{
// Same feature, skip doubles
continue;
}
if((int)hessianMap.size() < maxKeypoints)
{
maxKeypoints = hessianMap.size();
}
float response = abs(cudaSiftData_->h_data[i].sharpness);
if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
{
continue;
}
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
keypoints.resize(maxKeypoints);
cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
int i = iter->second;
float *desc = cudaSiftData_->h_data[i].data;
cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
keypoints[k].response = abs(cudaSiftData_->h_data[i].sharpness);
keypoints[k].response = response;
keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
++k;
}
if(k < keypoints.size())
{
UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
keypoints.resize(k);
cudaSiftDescriptors_.resize(k);
}
if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
{
// Call limitKeypoints() now to filter the descriptors.
this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
}
}
}
@@ -1457,12 +1465,13 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
cv::Mat descriptors;
#ifdef RTABMAP_CUDASIFT
if(gpu_)
{
if((int)keypoints.size() == cudaSiftDescriptors_.rows)
{
return cudaSiftDescriptors_.clone();
descriptors = cudaSiftDescriptors_.clone();
}
else
{
@@ -1470,19 +1479,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
return cv::Mat();
}
}
else
{
#endif
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
#ifdef RTABMAP_NONFREE
sift_->compute(image, keypoints, descriptors);
sift_->compute(image, keypoints, descriptors);
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
#else // >=4.4, >=3.4.11
sift_->compute(image, keypoints, descriptors);
sift_->compute(image, keypoints, descriptors);
#endif
#ifdef RTABMAP_CUDASIFT
}
#endif
if( rootSIFT_ && !descriptors.empty())
{
UDEBUG("Performing RootSIFT...");

34
corelib/src/Memory.cpp
View File

@@ -5168,16 +5168,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
UASSERT(!decimatedData.cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}
@@ -5227,6 +5218,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else
{
int oldMaxFeatures = _feature2D->getMaxFeatures();
bool oldSSC = _feature2D->getSSC();
UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
ParametersMap tmpMaxFeatureParameter;
if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
@@ -5234,6 +5226,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// The total extracted features should match the number of features used for transformation estimation
UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpSSC(), uNumber2Str(_visSSC)));
_feature2D->parseParameters(tmpMaxFeatureParameter);
}
@@ -5244,6 +5237,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(tmpMaxFeatureParameter.size())
{
tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
tmpMaxFeatureParameter.at(Parameters::kKpSSC()) = uBool2Str(oldSSC);
_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
}
t = timer.ticks();
@@ -5444,8 +5438,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if((int)keypoints.size() > 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);
if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
else
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
}
@@ -5678,13 +5672,17 @@ 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());
}
if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
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::limitKeypoints(
keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
_feature2D->getSSC());
}
else

22
corelib/src/RegistrationVis.cpp
View File

@@ -447,16 +447,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
UASSERT(!fromSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}
@@ -817,16 +808,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
UASSERT(!toSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}

1
corelib/src/util2d.cpp
View File

@@ -2296,6 +2296,7 @@ std::vector<int> SSC(
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();
maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints
// several temp expression variables to simplify solution equation
int exp1 = rows + cols + 2*maxKeypoints;