mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-11 22:10:21 +08:00
Cudasift tuning and SSC supporting multicameras (#1677)
* 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
@@ -309,7 +309,8 @@ private:
|
||||
bool preciseUpscale_;
|
||||
bool rootSIFT_;
|
||||
bool gpu_;
|
||||
float guaussianThreshold_;
|
||||
float gaussianThreshold_;
|
||||
float maxGaussianThreshold_;
|
||||
bool upscale_;
|
||||
|
||||
cv::Ptr<CV_SIFT> sift_;
|
||||
|
||||
@@ -224,7 +224,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
|
||||
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
|
||||
RTABMAP_PARAM(Mem, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary.");
|
||||
RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM(Mem, StereoFromMotion, bool, false, uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM(Mem, ImagePreDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM(Mem, ImagePostDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
|
||||
@@ -294,7 +294,8 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(SIFT, PreciseUpscale, bool, false, "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
|
||||
RTABMAP_PARAM(SIFT, RootSIFT, bool, false, "Apply RootSIFT normalization of the descriptors.");
|
||||
RTABMAP_PARAM(SIFT, Gpu, bool, false, "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
|
||||
RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
|
||||
RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.");
|
||||
RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0, uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str()));
|
||||
RTABMAP_PARAM(SIFT, Upscale, bool, false, "CudaSift: Whether to enable upscaling.");
|
||||
|
||||
RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
|
||||
@@ -724,7 +725,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
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.");
|
||||
RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
|
||||
RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
|
||||
RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
|
||||
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
|
||||
|
||||
+56
-41
@@ -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...");
|
||||
|
||||
+16
-18
@@ -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
|
||||
|
||||
@@ -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.");
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -6163,6 +6163,14 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
|
||||
kptB->keypoint().pt.y,
|
||||
cB);
|
||||
}
|
||||
else if(ids[i]<0)
|
||||
{
|
||||
ui_->graphicsView_A->setFeatureColor(ids[i], Qt::gray);
|
||||
}
|
||||
}
|
||||
for(auto iter = wordsB.begin(); iter.key()<0 && iter!=wordsB.end(); ++iter)
|
||||
{
|
||||
ui_->graphicsView_B->setFeatureColor(iter.key(), Qt::gray);
|
||||
}
|
||||
ui_->graphicsView_A->update();
|
||||
ui_->graphicsView_B->update();
|
||||
|
||||
@@ -1255,11 +1255,11 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, c
|
||||
{
|
||||
if (xRatio > 0 && yRatio > 0)
|
||||
{
|
||||
addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), color);
|
||||
addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), iter->first<0?Qt::gray:color);
|
||||
}
|
||||
else
|
||||
{
|
||||
addFeature(iter->first, iter->second, 0, color);
|
||||
addFeature(iter->first, iter->second, 0, iter->first<0?Qt::gray:color);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -171,6 +171,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
||||
_ui->sift_label_gpu->setEnabled(false);
|
||||
_ui->sift_doubleSpinBox_gaussianDiffThreshold->setEnabled(false);
|
||||
_ui->sift_label_gaussianThreshold->setEnabled(false);
|
||||
_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setEnabled(false);
|
||||
_ui->sift_label_maxGaussianThreshold->setEnabled(false);
|
||||
_ui->sift_checkBox_upscale->setEnabled(false);
|
||||
_ui->sift_label_upscale->setEnabled(false);
|
||||
#endif
|
||||
@@ -1145,6 +1147,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
||||
_ui->sift_checkBox_rootsift->setObjectName(Parameters::kSIFTRootSIFT().c_str());
|
||||
_ui->sift_checkBox_gpu->setObjectName(Parameters::kSIFTGpu().c_str());
|
||||
_ui->sift_doubleSpinBox_gaussianDiffThreshold->setObjectName(Parameters::kSIFTGaussianThreshold().c_str());
|
||||
_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setObjectName(Parameters::kSIFTMaxGaussianThreshold().c_str());
|
||||
_ui->sift_checkBox_upscale->setObjectName(Parameters::kSIFTUpscale().c_str());
|
||||
|
||||
//BRIEF descriptor
|
||||
|
||||
@@ -63,9 +63,9 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-2161</y>
|
||||
<y>-96</y>
|
||||
<width>684</width>
|
||||
<height>5201</height>
|
||||
<height>5218</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_16">
|
||||
@@ -95,7 +95,7 @@
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>5</number>
|
||||
<number>9</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_22">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
|
||||
@@ -11659,7 +11659,7 @@ generate the number of words requested.</string>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="label_591">
|
||||
<property name="text">
|
||||
<string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
|
||||
<string>Filter floor from depth mask. 0 means disabled.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
@@ -23860,7 +23860,7 @@ Lower the ratio -> higher the precision.</string>
|
||||
<item row="7" column="1">
|
||||
<widget class="QLabel" name="label_759">
|
||||
<property name="text">
|
||||
<string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
|
||||
<string>Filter floor from depth mask. 0 means disabled.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
@@ -26733,39 +26733,6 @@ Lower the ratio -> higher the precision.</string>
|
||||
<string>SIFT</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_19" columnstretch="0,1">
|
||||
<item row="4" column="0">
|
||||
<widget class="QCheckBox" name="sift_checkBox_preciseUpscale">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_30">
|
||||
<property name="text">
|
||||
<string>Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QSpinBox" name="sift_spinBox_nOctaveLayers"/>
|
||||
</item>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="sift_label_preciseUpscale">
|
||||
<property name="text">
|
||||
<string>Whether to enable precise upscaling in the scale pyramid.</string>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_130">
|
||||
<property name="text">
|
||||
@@ -26779,13 +26746,53 @@ Lower the ratio -> higher the precision.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QDoubleSpinBox" name="sift_doubleSpinBox_edgeThr">
|
||||
<property name="singleStep">
|
||||
<double>0.100000000000000</double>
|
||||
<item row="6" column="0">
|
||||
<widget class="QCheckBox" name="sift_checkBox_gpu">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>10.000000000000000</double>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="sift_label_preciseUpscale">
|
||||
<property name="text">
|
||||
<string>Whether to enable precise upscaling in the scale pyramid.</string>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="7" column="1">
|
||||
<widget class="QLabel" name="sift_label_gaussianThreshold">
|
||||
<property name="text">
|
||||
<string>CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.</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="QCheckBox" name="sift_checkBox_upscale">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="text">
|
||||
<string>Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -26796,6 +26803,42 @@ Lower the ratio -> higher the precision.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="0">
|
||||
<widget class="QCheckBox" name="sift_checkBox_preciseUpscale">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="1">
|
||||
<widget class="QLabel" name="sift_label_gpu">
|
||||
<property name="text">
|
||||
<string>CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QSpinBox" name="sift_spinBox_nOctaveLayers"/>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_129">
|
||||
<property name="text">
|
||||
<string>Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QDoubleSpinBox" name="sift_doubleSpinBox_contrastThr">
|
||||
<property name="decimals">
|
||||
@@ -26825,23 +26868,20 @@ Lower the ratio -> higher the precision.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="1">
|
||||
<widget class="QLabel" name="sift_label_gpu">
|
||||
<item row="9" column="1">
|
||||
<widget class="QLabel" name="sift_label_upscale">
|
||||
<property name="text">
|
||||
<string>CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
<string>CudaSift: Whether to enable upscaling.</string>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_30">
|
||||
<property name="text">
|
||||
<string>Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift.</string>
|
||||
<string>Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
@@ -26870,6 +26910,16 @@ Lower the ratio -> higher the precision.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QDoubleSpinBox" name="sift_doubleSpinBox_edgeThr">
|
||||
<property name="singleStep">
|
||||
<double>0.100000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>10.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="1">
|
||||
<widget class="QLabel" name="sift_label_rootsift">
|
||||
<property name="text">
|
||||
@@ -26880,43 +26930,13 @@ Lower the ratio -> higher the precision.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="7" column="1">
|
||||
<widget class="QLabel" name="sift_label_gaussianThreshold">
|
||||
<property name="text">
|
||||
<string>CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="6" column="0">
|
||||
<widget class="QCheckBox" name="sift_checkBox_gpu">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_129">
|
||||
<property name="text">
|
||||
<string>Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="8" column="1">
|
||||
<widget class="QLabel" name="sift_label_upscale">
|
||||
<widget class="QLabel" name="sift_label_maxGaussianThreshold">
|
||||
<property name="text">
|
||||
<string>CudaSift: Whether to enable upscaling.</string>
|
||||
<string>CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than gaussian threshold above). The lower the threshold, the less features with high response/hessian are produced by the detector.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
@@ -26924,9 +26944,21 @@ Lower the ratio -> higher the precision.</string>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="8" column="0">
|
||||
<widget class="QCheckBox" name="sift_checkBox_upscale">
|
||||
<property name="text">
|
||||
<string/>
|
||||
<widget class="QDoubleSpinBox" name="sift_doubleSpinBox_maxGaussianDiffThreshold">
|
||||
<property name="decimals">
|
||||
<number>2</number>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<double>0.010000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>100.000000000000000</double>
|
||||
</property>
|
||||
<property name="singleStep">
|
||||
<double>0.100000000000000</double>
|
||||
</property>
|
||||
<property name="value">
|
||||
<double>4.500000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
@@ -128,6 +128,8 @@ void sighandler(int sig)
|
||||
int loopCount = 0;
|
||||
int proxCount = 0;
|
||||
int loopCountMotion = 0;
|
||||
int loopInter = 0;
|
||||
int loopIntra = 0;
|
||||
int totalFrames = 0;
|
||||
int totalFramesMotion = 0;
|
||||
std::vector<float> previousLocalizationDistances;
|
||||
@@ -1239,15 +1241,23 @@ int main(int argc, char * argv[])
|
||||
++loopCountMotion;
|
||||
}
|
||||
int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
|
||||
printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
|
||||
if(loopMapId != stats.refImageMapId())
|
||||
{
|
||||
++loopInter;
|
||||
}
|
||||
else
|
||||
{
|
||||
++loopIntra;
|
||||
}
|
||||
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
|
||||
}
|
||||
else if(landmarkId != 0)
|
||||
{
|
||||
printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000), landmarkId);
|
||||
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), landmarkId);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000));
|
||||
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
|
||||
}
|
||||
|
||||
// Here we accumulate statistics about distance from last localization
|
||||
@@ -1340,7 +1350,8 @@ int main(int argc, char * argv[])
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
|
||||
printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d, Intra=%d, Inter=%d)\n",
|
||||
loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion, loopIntra, loopInter);
|
||||
|
||||
if(databases.size()>1)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user