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

Optimizing the graph if gravity or prior constraints are added in SLAM mode

Browse Source
This commit is contained in:
matlabbe
2019-09-12 18:56:01 -04:00
parent 5b6eddabde
commit bdc8d839ff
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/Rtabmap.cpp
View File

@@ -2560,8 +2560,8 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
(signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored() && !_memory->isIncremental()) || // prior edge (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isIncremental()) || // gravity edge (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
@@ -2839,7 +2839,9 @@ bool Rtabmap::process(
{ {
if( iter->second.type() != Link::kNeighbor && if( iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kVirtualClosure && iter->second.type() != Link::kVirtualClosure &&
iter->second.type() != Link::kLandmark) iter->second.type() != Link::kLandmark &&
iter->second.type() != Link::kGravity &&
iter->second.type() != Link::kPosePrior)
{ {
UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection); UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
poses.clear(); poses.clear();