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

Added parameters Vis/MeanInliersDistance and Vis/MinInliersDistribution

Browse Source
This commit is contained in:
matlabbe
2019-06-16 18:54:39 -04:00
parent 85edc57ba5
commit d205683eb5
13 changed files with 206 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/Parameters.cpp
View File

@@ -237,6 +237,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.19.4
removedParameters_.insert(std::make_pair("RGBD/MaxLocalizationDistance", std::make_pair(true, Parameters::kRGBDMaxLoopClosureDistance())));
// 0.19.3
removedParameters_.insert(std::make_pair("Aruco/Dictionary", std::make_pair(true, Parameters::kMarkerDictionary())));
removedParameters_.insert(std::make_pair("Aruco/MarkerLength", std::make_pair(true, Parameters::kMarkerLength())));

101
corelib/src/RegistrationVis.cpp
View File

@@ -68,7 +68,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
_depthAsMask(Parameters::defaultVisDepthAsMask())
_depthAsMask(Parameters::defaultVisDepthAsMask()),
_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -112,6 +114,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
uInsert(_bundleParameters, parameters);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -555,6 +559,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
imageFrom = tmp;
}
UDEBUG("cleared orignalWordsFromIds");
orignalWordsFromIds.clear();
descriptorsFrom = detectorFrom->generateDescriptors(imageFrom, kptsFrom);
}
@@ -729,6 +734,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
UDEBUG("orignalWordsFromIds=%d", (int)orignalWordsFromIds.size());
// We have all data we need here, so match!
if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
@@ -1632,6 +1638,99 @@ Transform RegistrationVis::computeTransformationImpl(
transform = transforms[0];
covariance = covariances[0];
}
if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
{
cv::Mat pcaData;
float cx=0, cy=0, w=0, h=0;
if(_minInliersDistributionThr > 0)
{
if(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
{
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
cx = cameraModel.cx();
cy = cameraModel.cy();
w = cameraModel.imageWidth();
h = cameraModel.imageHeight();
if(w>0 && h>0)
{
pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
}
else
{
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
else if(toSignature.sensorData().cameraModels().size() > 1)
{
UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
else
{
UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
Transform transformInv = transform.inverse();
std::vector<float> distances;
if(_maxInliersMeanDistance>0.0f)
{
distances.reserve(allInliers.size());
}
for(unsigned int i=0; i<allInliers.size(); ++i)
{
if(_maxInliersMeanDistance>0.0f)
{
std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
if(words3Iter != fromSignature.getWords3().end())
{
if(uIsFinite(words3Iter->second.x))
{
cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
distances.push_back(pt.x);
}
}
}
if(!pcaData.empty())
{
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end());
float * ptr = pcaData.ptr<float>(i, 0);
ptr[0] = (wordsIter->second.pt.x-cx) / w;
ptr[1] = (wordsIter->second.pt.y-cy) / h;
}
}
if(!distances.empty())
{
info.inliersMeanDistance = uMean(distances);
if(info.inliersMeanDistance > _maxInliersMeanDistance)
{
msg = uFormat("The mean distance of the inliers is over %s threshold (%f)",
info.inliersMeanDistance, Parameters::kVisMeanInliersDistance().c_str(), _maxInliersMeanDistance);
transform.setNull();
}
}
if(!transform.isNull() && !pcaData.empty())
{
cv::Mat pcaEigenVectors, pcaEigenValues;
cv::PCA pca_analysis(pcaData, cv::Mat(), CV_PCA_DATA_AS_ROW);
// We take the second eigen value
info.inliersDistribution = pca_analysis.eigenvalues.at<float>(0, 1);
if(info.inliersDistribution < _minInliersDistributionThr)
{
msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)",
info.inliersDistribution, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
transform.setNull();
}
}
}
}
else if(toSignature.sensorData().isValid())
{

32
corelib/src/Rtabmap.cpp
View File

@@ -87,7 +87,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_localizationMaxDistance(Parameters::defaultRGBDMaxLocalizationDistance()),
_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
@@ -442,7 +442,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalizationDistance(), _localizationMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
@@ -2128,6 +2128,8 @@ bool Rtabmap::process(
int loopClosureVisualMatches = 0;
float loopClosureLinearVariance = 0.0f;
float loopClosureAngularVariance = 0.0f;
float loopClosureVisualInliersMeanDist = 0;
float loopClosureVisualInliersDistribution = 0;
if(_loopClosureHypothesis.first>0)
{
//Compute transform if metric data are present
@@ -2137,6 +2139,9 @@ bool Rtabmap::process(
if(_rgbdSlamMode)
{
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
loopClosureVisualInliersDistribution = info.inliersDistribution;
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
rejectedHypothesis = transform.isNull();
@@ -2145,11 +2150,11 @@ bool Rtabmap::process(
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
else if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && transform.getNorm() > _localizationMaxDistance)
else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
{
rejectedHypothesis = true;
UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLocalizationDistance().c_str(), _localizationMaxDistance);
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
}
else
{
@@ -2275,9 +2280,9 @@ bool Rtabmap::process(
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
float proximityFilteringRadius = _proximityFilteringRadius;
if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && (proximityFilteringRadius <= 0.0f || _localizationMaxDistance<proximityFilteringRadius))
if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
{
proximityFilteringRadius = _localizationMaxDistance;
proximityFilteringRadius = _maxLoopClosureDistance;
}
for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
@@ -2330,6 +2335,12 @@ bool Rtabmap::process(
if(_loopClosureHypothesis.first == 0)
{
if(proximityDetectionsAddedVisually == 0)
{
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
loopClosureVisualInliersDistribution = info.inliersDistribution;
}
++proximityDetectionsAddedVisually;
lastProximitySpaceClosureId = nearestId;
@@ -3052,6 +3063,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
@@ -3067,6 +3080,13 @@ bool Rtabmap::process(
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().find(signature->id())->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().find(signature->id())->second.transform());
statistics_.addStatistic(Statistics::kLoopMap_correction_norm(), _mapCorrection.getNorm());
float roll,pitch,yaw;
_mapCorrection.getEulerAngles(roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMap_correction_roll(), roll*180/M_PI);
statistics_.addStatistic(Statistics::kLoopMap_correction_pitch(), pitch*180/M_PI);
statistics_.addStatistic(Statistics::kLoopMap_correction_yaw(), yaw*180/M_PI);
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());