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

Fixed graph deformation check without landmark's orientation optimized. Added parameter Marker/VarianceOrientationIgnored to be able to tune correctly GTSAM's bearing/range factor variance.

Browse Source
This commit is contained in:
matlabbe
2025-03-22 20:43:27 -07:00
parent 5f946cc26b
commit 68e3125358
10 changed files with 263 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+21 -25
View File
@@ -1241,7 +1241,7 @@ bool Rtabmap::process(
double timeStatsCreation = 0;
float hypothesisRatio = 0.0f; // Only used for statistics
bool rejectedGlobalLoopClosure = false;
bool rejectedLoopClosure = false;
std::map<int, float> rawLikelihood;
std::map<int, float> adjustedLikelihood;
@@ -2159,7 +2159,7 @@ bool Rtabmap::process(
// Loop closure Threshold
if(_highestHypothesis.second >= loopThr)
{
rejectedGlobalLoopClosure = true;
rejectedLoopClosure = true;
if(posterior.size() <= 2 && loopThr>0.0f)
{
// Ignore loop closure if there is only one loop closure hypothesis
@@ -2181,7 +2181,7 @@ bool Rtabmap::process(
else
{
_loopClosureHypothesis = _highestHypothesis;
rejectedGlobalLoopClosure = false;
rejectedLoopClosure = false;
}
timeHypothesesValidation = timer.ticks();
@@ -2192,7 +2192,7 @@ bool Rtabmap::process(
// Used for Precision-Recall computation.
// When analyzing logs, it's convenient to know
// if the hypothesis would be rejected if T_loop would be lower.
rejectedGlobalLoopClosure = true;
rejectedLoopClosure = true;
UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
}
@@ -3061,15 +3061,15 @@ bool Rtabmap::process(
loopClosureVisualInliers = info.inliers;
loopClosureVisualInliersRatio = info.inliersRatio;
loopClosureVisualMatches = info.matches;
rejectedGlobalLoopClosure = transform.isNull();
if(rejectedGlobalLoopClosure)
rejectedLoopClosure = transform.isNull();
if(rejectedLoopClosure)
{
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
{
rejectedGlobalLoopClosure = true;
rejectedLoopClosure = true;
UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
}
@@ -3078,7 +3078,7 @@ bool Rtabmap::process(
transform = transform.inverse();
}
}
if(!rejectedGlobalLoopClosure)
if(!rejectedLoopClosure)
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
@@ -3086,14 +3086,14 @@ bool Rtabmap::process(
loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
cv::Mat information = getInformation(info.covariance);
rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedGlobalLoopClosure)
rejectedLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedLoopClosure)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
}
}
if(rejectedGlobalLoopClosure)
if(rejectedLoopClosure)
{
_loopClosureHypothesis.first = 0;
}
@@ -3137,7 +3137,7 @@ bool Rtabmap::process(
{
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
rejectedLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first));
}
}
@@ -3180,7 +3180,6 @@ bool Rtabmap::process(
double optimizationError = 0.0;
int optimizationIterations = 0;
Transform previousMapCorrection;
bool rejectedLandmark = false;
bool delayedLocalization = false;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
@@ -3768,8 +3767,7 @@ bool Rtabmap::process(
{
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedGlobalLoopClosure = true;
rejectedLandmark = true;
rejectedLoopClosure = true;
}
}
else
@@ -3804,8 +3802,7 @@ bool Rtabmap::process(
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedGlobalLoopClosure = true;
rejectedLandmark = true;
rejectedLoopClosure = true;
}
else if(_memory->isIncremental() &&
loopClosureLinksAdded.size() &&
@@ -3915,8 +3912,7 @@ bool Rtabmap::process(
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedGlobalLoopClosure = true;
rejectedLandmark = true;
rejectedLoopClosure = true;
}
}
@@ -4077,7 +4073,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
float x,y,z,roll,pitch,yaw;
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLoopClosure && !landmarksDetected.empty()))
{
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{
@@ -4179,7 +4175,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
//Epipolar geometry constraint
statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedGlobalLoopClosure?1.0f:0);
statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedLoopClosure?1.0f:0);
statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
@@ -4274,7 +4270,7 @@ bool Rtabmap::process(
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
(landmarksDetected.empty() || rejectedLandmark) && // if we re not seeing a landmark from a previous map
(landmarksDetected.empty() || rejectedLoopClosure) && // if we re not seeing a landmark from a previous map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -4294,7 +4290,7 @@ bool Rtabmap::process(
else if((smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
(rejectedLandmark || landmarksDetected.empty()) &&
(rejectedLoopClosure || landmarksDetected.empty()) &&
!addedNewLandmark)
{
// Don't delete the location if a loop closure is detected
@@ -4314,7 +4310,7 @@ bool Rtabmap::process(
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
!delayedLocalization &&
(rejectedLandmark || landmarksDetected.empty()))
(rejectedLoopClosure || landmarksDetected.empty()))
{
_odomCachePoses.erase(signatureRemoved);
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
@@ -4722,7 +4718,7 @@ bool Rtabmap::process(
refWordsCount,
dictionarySize,
int(_memory->getWorkingMem().size()),
rejectedGlobalLoopClosure?1:0,
rejectedLoopClosure?1:0,
0,
0,
int(signaturesRetrieved.size()),