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

Rtabmap: re-optimize at each frame when gravity constraints are added

Browse Source
This commit is contained in:
matlabbe
2019-05-31 19:31:11 -04:00
parent 4c8af6d6e9
commit ec943198f0
4 changed files with 26 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+6 -1
View File
@@ -2533,7 +2533,8 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
(signature->hasLink(signature->id()) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
@@ -2720,6 +2721,10 @@ bool Rtabmap::process(
u = signature->getPose() * transform;
}
else
{
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
}
}
Transform up = u * oldPose.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)