i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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()));
_stMem.erase(*_stMem.begin());
++_signaturesAdded;
}
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));
_stMem.insert(_stMem.end(), signature->id());
++_signaturesAdded;
if(_vwd)
{
@@ -1485,7 +1485,8 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
{
UDEBUG("");
// 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)
{
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
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;
}
@@ -1513,10 +1522,6 @@ int Memory::cleanup()
}
signatureRemoved = _lastSignature->id();
moveToTrash(_lastSignature, _incrementalMemory);
if(_signaturesAdded>0)
{
--_signaturesAdded;
}
}
return signatureRemoved;
@@ -1656,42 +1661,39 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
int recentWmCount = 0;
// make the list of removable signatures
// 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();
iter!=weightAgeIdMap.end();
++iter)
{
bool removable = true;
if(removable)
if(!recentWmImmunized)
{
if(!recentWmImmunized)
{
UDEBUG("weight=%d, id=%d",
iter->second->getWeight(),
iter->second->id());
removableSignatures.push_back(iter->second);
UDEBUG("weight=%d, id=%d",
iter->second->getWeight(),
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;
if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
{
UDEBUG("switched recentWmImmunized");
recentWmImmunized = true;
}
UDEBUG("switched recentWmImmunized");
recentWmImmunized = true;
}
}
else if(iter->second->id() < _lastGlobalLoopClosureId)
{
UDEBUG("weight=%d, id=%d",
iter->second->getWeight(),
iter->second->id());
removableSignatures.push_back(iter->second);
}
if(removableSignatures.size() >= (unsigned int)count)
{
break;
}
}
else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId)
{
UDEBUG("weight=%d, id=%d",
iter->second->getWeight(),
iter->second->id());
removableSignatures.push_back(iter->second);
}
if(removableSignatures.size() >= (unsigned int)count)
{
break;
}
}
}
@@ -1778,6 +1780,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
_workingMem.erase(s->id());
_stMem.erase(s->id());
_signatures.erase(s->id());
if(_signaturesAdded>0)
{
--_signaturesAdded;
}
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());
}
updateGoalIndex();
return true;
return _path.size() || _pathStatus > 0;
}
return false;
@@ -3450,6 +3449,8 @@ void Rtabmap::updateGoalIndex()
}
}
// 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());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT(currentIndexS != 0);