i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated version to 0.8.6: Added parameter Mem/TransferSortingByWeightId

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-09 15:25:51 -04:00
parent 49fcf732af
commit d77b0dfb14
8 changed files with 115 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/Memory.h
View File

@@ -73,6 +73,7 @@ public:
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId();
void updateAge(int signatureId);
std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
@@ -93,7 +94,7 @@ public:
void removeLink(int idA, int idB);
//getters
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::map<int, double> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
std::map<int, Link> getNeighborLinks(int signatureId,
@@ -212,6 +213,7 @@ private:
bool _incrementalMemory;
int _maxStMemSize;
float _recentWmRatio;
bool _transferSortingByWeightId;
bool _idUpdatedToNewOneRehearsal;
bool _generateIds;
bool _badSignaturesIgnored;
@@ -231,7 +233,7 @@ private:
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
std::set<int> _workingMem; // id,age
std::map<int, double> _workingMem; // id,age
//Keypoint stuff
VWDictionary * _vwd;

7
corelib/include/rtabmap/core/Parameters.h
View File

@@ -185,12 +185,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity.");
RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM.");
RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db.");
RTABMAP_PARAM(Mem, RehearsedNodesKept, bool, true, "Keep rehearsed ndoes in db.");
RTABMAP_PARAM(Mem, RehearsedNodesKept, bool, true, "Keep rehearsed nodes in db.");
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, othwersize it is Localization mode.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location Ids, False=use input image ids.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1).");

97
corelib/src/Memory.cpp
View File

@@ -63,6 +63,7 @@ Memory::Memory(const ParametersMap & parameters) :
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
_transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()),
_idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
@@ -205,7 +206,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
// only linked with the ones of the current session by
// global loop closures.
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
_workingMem.insert((*iter)->id());
_workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
}
else
{
@@ -221,7 +222,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
else if(_workingMem.size()>0)
{
_lastSignature = uValue(_signatures, *_workingMem.rbegin(), (Signature*)0);
_lastSignature = uValue(_signatures, _workingMem.rbegin()->first, (Signature*)0);
}
// Last id
@@ -239,7 +240,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
_idMapCount = kIdStart;
}
_workingMem.insert(kIdVirtual);
_workingMem.insert(std::make_pair(kIdVirtual, 0));
UDEBUG("ids start with %d", _idCount+1);
UDEBUG("map ids start with %d", _idMapCount);
@@ -384,6 +385,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
@@ -607,7 +609,7 @@ bool Memory::update(const SensorData & data, Statistics * stats)
}
}
}
_workingMem.insert(_workingMem.end(), *_stMem.begin());
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
++_signaturesAdded;
}
@@ -729,7 +731,7 @@ void Memory::addSignatureToWm(Signature * signature)
if(signature)
{
UDEBUG("Inserting node %d in WM...", signature->id());
_workingMem.insert(signature->id());
_workingMem.insert(std::make_pair(signature->id(), UTimer::now()));
_signatures.insert(std::pair<int, Signature*>(signature->id(), signature));
++_signaturesAdded;
}
@@ -952,7 +954,7 @@ int Memory::incrementMapId()
}
}
}
_workingMem.insert(_workingMem.end(), *_stMem.begin());
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
}
@@ -961,6 +963,15 @@ int Memory::incrementMapId()
return _idMapCount;
}
void Memory::updateAge(int signatureId)
{
std::map<int, double>::iterator iter=_workingMem.find(signatureId);
if(iter!=_workingMem.end())
{
iter->second = UTimer::now();
}
}
int Memory::getDatabaseMemoryUsed() const
{
int memoryUsed = 0;
@@ -1006,7 +1017,7 @@ void Memory::clear()
if(_dbDriver && (_stMem.size() || _workingMem.size()))
{
unsigned int memSize = _workingMem.size() + _stMem.size();
if(_workingMem.size() && *_workingMem.begin() < 0)
if(_workingMem.size() && _workingMem.begin()->first < 0)
{
--memSize;
}
@@ -1039,7 +1050,7 @@ void Memory::clear()
}
}
if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && *_workingMem.begin() == kIdVirtual))
if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual))
{
ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
}
@@ -1211,20 +1222,20 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
std::map<int, int> Memory::getWeights() const
{
std::map<int, int> weights;
for(std::set<int>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
for(std::map<int, double>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
{
if(*iter > 0)
if(iter->first > 0)
{
const Signature * s = this->getSignature(*iter);
const Signature * s = this->getSignature(iter->first);
if(!s)
{
UFATAL("Location %d must exist in memory", *iter);
UFATAL("Location %d must exist in memory", iter->first);
}
weights.insert(weights.end(), std::make_pair(*iter, s->getWeight()));
weights.insert(weights.end(), std::make_pair(iter->first, s->getWeight()));
}
else
{
weights.insert(weights.end(), std::make_pair(*iter, -1));
weights.insert(weights.end(), std::make_pair(iter->first, -1));
}
}
return weights;
@@ -1323,13 +1334,14 @@ void Memory::joinTrashThread()
}
}
class WeightIdKey
class WeightAgeIdKey
{
public:
WeightIdKey(int w, int i) :
WeightAgeIdKey(int w, double a, int i) :
weight(w),
id(i) {}
bool operator<(const WeightIdKey & k) const
age(a),
id(i){}
bool operator<(const WeightAgeIdKey & k) const
{
if(weight < k.weight)
{
@@ -1337,35 +1349,41 @@ public:
}
else if(weight == k.weight)
{
if(id < k.id)
if(age < k.age)
{
return true;
}
else if(age == k.age)
{
if(id < k.id)
{
return true;
}
}
}
return false;
}
int weight, id;
int weight, age, id;
};
std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
{
//UDEBUG("");
std::list<Signature *> removableSignatures;
std::map<WeightIdKey, Signature *> signatureMap;
std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
// Find the last index to check...
const std::set<int> & wm = _workingMem;
UDEBUG("mem.size()=%d, ignoredIds.size()=%d", wm.size(), ignoredIds.size());
UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size());
if(wm.size())
if(_workingMem.size())
{
int recentWmMaxSize = _recentWmRatio * float(wm.size());
int recentWmMaxSize = _recentWmRatio * float(_workingMem.size());
bool recentWmImmunized = false;
// look for the position of the lastLoopClosureId in WM
int currentRecentWmSize = 0;
if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
{
// If set, it must be in WM
std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
std::map<int, double>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
while(iter != _workingMem.end())
{
++currentRecentWmSize;
@@ -1389,16 +1407,16 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
lastInSTM = _signatures.at(*_stMem.begin());
}
for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
for(std::map<int, double>::const_iterator memIter = _workingMem.begin(); memIter != _workingMem.end(); ++memIter)
{
if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureId) ||
*memIter == _lastGlobalLoopClosureId)
if( (recentWmImmunized && memIter->first > _lastGlobalLoopClosureId) ||
memIter->first == _lastGlobalLoopClosureId)
{
// ignore recent memory
}
else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(*memIter)))
else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(memIter->first)))
{
Signature * s = this->_getSignature(*memIter);
Signature * s = this->_getSignature(memIter->first);
if(s)
{
// Links must not be in STM to be removable, rehearsal issue
@@ -1415,7 +1433,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
if(!foundInSTM)
{
// less weighted signature priority to be transferred
signatureMap.insert(std::make_pair(WeightIdKey(s->getWeight(), s->id()), s));
weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s));
}
}
else
@@ -1430,12 +1448,11 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
}
int recentWmCount = 0;
std::set<int> addedSignatures;
// make the list of removable signatures
// Criteria : Weight -> ID
UDEBUG("signatureMap.size()=%d", (int)signatureMap.size());
for(std::map<WeightIdKey, Signature*>::iterator iter=signatureMap.begin();
iter!=signatureMap.end();
UDEBUG("signatureMap.size()=%d", (int)weightAgeIdMap.size());
for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
iter!=weightAgeIdMap.end();
++iter)
{
bool removable = true;
@@ -1444,10 +1461,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
if(!recentWmImmunized)
{
UDEBUG("weight=%d, id=%d",
iter->first.weight,
iter->second->getWeight(),
iter->second->id());
removableSignatures.push_back(iter->second);
addedSignatures.insert(iter->second->id());
if(iter->second->id() > _lastGlobalLoopClosureId)
{
@@ -1462,10 +1478,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
else if(iter->second->id() < _lastGlobalLoopClosureId)
{
UDEBUG("weight=%d, id=%d",
iter->first.weight,
iter->second->getWeight(),
iter->second->id());
removableSignatures.push_back(iter->second);
addedSignatures.insert(iter->second->id());
}
if(removableSignatures.size() >= (unsigned int)count)
{
@@ -1584,7 +1599,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
}
else if(_workingMem.size())
{
_lastSignature = this->_getSignature(*_workingMem.rbegin());
_lastSignature = this->_getSignature(_workingMem.rbegin()->first);
}
}

18
corelib/src/Rtabmap.cpp
View File

@@ -462,7 +462,7 @@ std::list<int> Rtabmap::getWM() const
std::list<int> mem;
if(_memory)
{
mem = std::list<int>(_memory->getWorkingMem().begin(), _memory->getWorkingMem().end());
mem = uKeysList(_memory->getWorkingMem());
mem.remove(-1);// Ignore the virtual signature (if here)
}
return mem;
@@ -991,9 +991,7 @@ bool Rtabmap::process(const SensorData & data)
// with all images contained in the working memory + reactivated.
//============================================================
ULOGGER_INFO("computing likelihood...");
const std::set<int> & wm = _memory->getWorkingMem();
std::list<int> signaturesToCompare(wm.begin(), wm.end());
std::list<int> signaturesToCompare = uKeysList(_memory->getWorkingMem());
rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
// Adjust the likelihood (with mean and std dev)
@@ -1309,6 +1307,11 @@ bool Rtabmap::process(const SensorData & data)
}
}
}
// update Age of the close signatures (oldest the farthest)
for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
{
_memory->updateAge(iter->second);
}
}
// insert them first to make sure they are loaded.
@@ -2257,8 +2260,7 @@ void Rtabmap::dumpPrediction() const
{
if(_memory && _bayesFilter)
{
const std::set<int> & wm = _memory->getWorkingMem();
cv::Mat prediction = _bayesFilter->generatePrediction(_memory, std::vector<int>(wm.begin(), wm.end()));
cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uKeys(_memory->getWorkingMem()));
FILE* fout = 0;
std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
@@ -2331,12 +2333,12 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
// Get data
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
//remove virtual signature
ids.erase(Memory::kIdVirtual);
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end()); // STM + WM
if(global)
{
ids = _memory->getAllSignatureIds(); // STM + WM + LTM