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

Fixed map::at() seg fault when localizing on a database with multiple disjoint sessions

Browse Source
This commit is contained in:
matlabbe
2022-09-13 06:19:23 +00:00
parent 0dc5fbdd70
commit 749cd096ff
2 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/Rtabmap.cpp
View File

@@ -2996,6 +2996,16 @@ bool Rtabmap::process(
}
}
bool allLocalizationLinksInGraph = !localizationLinks.empty();
for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
{
if(!uContains(_optimizedPoses, iter->first))
{
allLocalizationLinksInGraph = false;
break;
}
}
// Note that in localization mode, we don't re-optimize the graph
// if:
// 1- there are no signatures retrieved,
@@ -3003,7 +3013,7 @@ bool Rtabmap::process(
if(!_memory->isIncremental() &&
signaturesRetrieved.empty() &&
!localizationLinks.empty() &&
uContains(_optimizedPoses, localizationLinks.rbegin()->first))
allLocalizationLinksInGraph)
{
bool rejectLocalization = _odomCachePoses.empty();
if(!_odomCachePoses.empty())
@@ -3311,6 +3321,7 @@ bool Rtabmap::process(
_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
}
_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
// At least 2 localizations at 2 different time required
@@ -3340,7 +3351,7 @@ bool Rtabmap::process(
!landmarksDetected.at(landmarkId).empty());
loopId = *landmarksDetected.at(landmarkId).begin();
}
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);