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

Memory: Fixed all recent memory immunized when lastGlobalLoopClosure is 0. Notify plan failed (instead of success) when the target node id is not found.

Browse Source
This commit is contained in:
matlabbe
2015-09-30 11:36:49 -04:00
parent e193aed720
commit 0c8309bb28
2 changed files with 43 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+40 -34
View File
@@ -666,7 +666,6 @@ bool Memory::update(
} }
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin()); _stMem.erase(*_stMem.begin());
++_signaturesAdded;
} }
if(!_memoryChanged && _incrementalMemory) if(!_memoryChanged && _incrementalMemory)
@@ -768,6 +767,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
_signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature)); _signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature));
_stMem.insert(_stMem.end(), signature->id()); _stMem.insert(_stMem.end(), signature->id());
++_signaturesAdded;
if(_vwd) if(_vwd)
{ {
@@ -1485,7 +1485,8 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
{ {
UDEBUG(""); UDEBUG("");
// Remove one more than total added during the iteration // Remove one more than total added during the iteration
std::list<Signature *> signatures = getRemovableSignatures(_signaturesAdded+1, ignoredIds); int signaturesAdded = _signaturesAdded;
std::list<Signature *> signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds);
for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{ {
signaturesRemoved.push_back((*iter)->id()); signaturesRemoved.push_back((*iter)->id());
@@ -1493,7 +1494,15 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
// and it is removed from the memory list // and it is removed from the memory list
this->moveToTrash(*iter); this->moveToTrash(*iter);
} }
UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), _signaturesAdded); if((int)signatures.size() < signaturesAdded)
{
UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
(int)signatures.size(), signaturesAdded);
}
else
{
UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded);
}
} }
return signaturesRemoved; return signaturesRemoved;
} }
@@ -1513,10 +1522,6 @@ int Memory::cleanup()
} }
signatureRemoved = _lastSignature->id(); signatureRemoved = _lastSignature->id();
moveToTrash(_lastSignature, _incrementalMemory); moveToTrash(_lastSignature, _incrementalMemory);
if(_signaturesAdded>0)
{
--_signaturesAdded;
}
} }
return signatureRemoved; return signatureRemoved;
@@ -1656,42 +1661,39 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
int recentWmCount = 0; int recentWmCount = 0;
// make the list of removable signatures // make the list of removable signatures
// Criteria : Weight -> ID // Criteria : Weight -> ID
UDEBUG("signatureMap.size()=%d", (int)weightAgeIdMap.size()); UDEBUG("signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d",
(int)weightAgeIdMap.size(), _lastGlobalLoopClosureId, currentRecentWmSize, recentWmMaxSize);
for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin(); for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
iter!=weightAgeIdMap.end(); iter!=weightAgeIdMap.end();
++iter) ++iter)
{ {
bool removable = true; if(!recentWmImmunized)
if(removable)
{ {
if(!recentWmImmunized) UDEBUG("weight=%d, id=%d",
{ iter->second->getWeight(),
UDEBUG("weight=%d, id=%d", iter->second->id());
iter->second->getWeight(), removableSignatures.push_back(iter->second);
iter->second->id());
removableSignatures.push_back(iter->second);
if(iter->second->id() > _lastGlobalLoopClosureId) if(_lastGlobalLoopClosureId && iter->second->id() > _lastGlobalLoopClosureId)
{
++recentWmCount;
if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
{ {
++recentWmCount; UDEBUG("switched recentWmImmunized");
if(currentRecentWmSize - recentWmCount < recentWmMaxSize) recentWmImmunized = true;
{
UDEBUG("switched recentWmImmunized");
recentWmImmunized = true;
}
} }
} }
else if(iter->second->id() < _lastGlobalLoopClosureId) }
{ else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId)
UDEBUG("weight=%d, id=%d", {
iter->second->getWeight(), UDEBUG("weight=%d, id=%d",
iter->second->id()); iter->second->getWeight(),
removableSignatures.push_back(iter->second); iter->second->id());
} removableSignatures.push_back(iter->second);
if(removableSignatures.size() >= (unsigned int)count) }
{ if(removableSignatures.size() >= (unsigned int)count)
break; {
} break;
} }
} }
} }
@@ -1778,6 +1780,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
_workingMem.erase(s->id()); _workingMem.erase(s->id());
_stMem.erase(s->id()); _stMem.erase(s->id());
_signatures.erase(s->id()); _signatures.erase(s->id());
if(_signaturesAdded>0)
{
--_signaturesAdded;
}
if(_lastSignature == s) if(_lastSignature == s)
{ {
corelib/src/Rtabmap.cpp
+3 -2
View File
@@ -3243,8 +3243,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
} }
updateGoalIndex(); updateGoalIndex();
return _path.size() || _pathStatus > 0;
return true;
} }
return false; return false;
@@ -3450,6 +3449,8 @@ void Rtabmap::updateGoalIndex()
} }
} }
// for the current index, only keep the newest virtual link // for the current index, only keep the newest virtual link
// This will make sure that the path is still connected even
// if the new signature is removed (e.g., because of a small displacement)
UASSERT(_pathCurrentIndex < _path.size()); UASSERT(_pathCurrentIndex < _path.size());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first); const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT(currentIndexS != 0); UASSERT(currentIndexS != 0);