mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-01 17:10:26 +08:00
TriggerNewMap: move all locations of STM in WM
This commit is contained in:
@@ -940,6 +940,37 @@ int Memory::incrementMapId()
|
||||
const Signature * s = getLastWorkingSignature();
|
||||
if(s && s->mapId() == _idMapCount)
|
||||
{
|
||||
// New session! move all signatures from the STM to WM
|
||||
while(_stMem.size())
|
||||
{
|
||||
UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
|
||||
if(!_localSpaceLinksKeptInWM)
|
||||
{
|
||||
// remove local space links outside STM
|
||||
Signature * s = this->_getSignature(*_stMem.begin());
|
||||
UASSERT(s!=0);
|
||||
std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
|
||||
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(iter->second.type() == Link::kLocalSpaceClosure)
|
||||
{
|
||||
Signature * sTo = this->_getSignature(iter->first);
|
||||
if(sTo)
|
||||
{
|
||||
sTo->removeLink(s->id());
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
|
||||
}
|
||||
s->removeLink(iter->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
_workingMem.insert(_workingMem.end(), *_stMem.begin());
|
||||
_stMem.erase(*_stMem.begin());
|
||||
}
|
||||
|
||||
return ++_idMapCount;
|
||||
}
|
||||
return _idMapCount;
|
||||
|
||||
Reference in New Issue
Block a user