i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

7
corelib/src/Rtabmap.cpp
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)

15
corelib/src/Signature.cpp
View File

@@ -126,9 +126,20 @@ void Signature::addLink(const Link & link)
_linksModified = true;
}
bool Signature::hasLink(int idTo) const
bool Signature::hasLink(int idTo, Link::Type type) const
{
return _links.find(idTo) != _links.end();
if(type == Link::kUndef)
{
return _links.find(idTo) != _links.end();
}
for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
{
if(type == iter->second.type())
{
return true;
}
}
return false;
}
void Signature::changeLinkIds(int idFrom, int idTo)