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

Added statistics : Rehearsal value, retrieval margin

Browse Source 
Updated the way how the retrieval margin is increased/reset
Posterior ignoring the last added location to WM
Auto save figure, removed the "-" in the generated date name


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@294 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-07-18 17:59:57 +00:00
parent e75f0bff66
commit 73a2d589ab
9 changed files with 115 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -52,6 +52,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Vp_likelihood,);
RTABMAP_STATS(Loop, ReactivateId,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Retrieval_margin,)
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);

32
corelib/src/KeypointMemory.cpp
View File

@@ -454,26 +454,30 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
const std::map<int, int> & wm = this->getWorkingMem();
for(std::map<int, int>::const_iterator iter = wm.begin(); iter!=wm.end(); ++iter)
{
likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
if(_tfIdfNormalized)
//ignore the last in WM
if(iter->first != wm.rbegin()->first)
{
const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
float wordsCountRatio = -1; // default invalid
if(s)
likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
if(_tfIdfNormalized)
{
if(s->getWords().size() > newSurf->getWords().size())
const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
float wordsCountRatio = -1; // default invalid
if(s)
{
wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
if(s->getWords().size() > newSurf->getWords().size())
{
wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
}
else if(newSurf->getWords().size())
{
wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
}
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
else if(newSurf->getWords().size())
else
{
wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
else
{
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
}
}

77
corelib/src/Memory.cpp
View File

@@ -236,20 +236,22 @@ void Memory::preUpdate()
}
}
bool Memory::update(const SMState * smState, std::list<std::pair<std::string, float> > & stats)
bool Memory::update(const SMState * smState, std::map<std::string, float> & stats)
{
ULOGGER_DEBUG("");
UTimer timer;
UTimer totalTimer;
timer.start();
float t;
//============================================================
// Pre update...
//============================================================
ULOGGER_DEBUG("pre-updating...");
this->preUpdate();
stats.push_back(std::pair<std::string, float>(std::string("TimingMem/Pre-update/ms"), timer.ticks()*1000));
ULOGGER_DEBUG("time preUpdate=%f ms", stats.back().second);
t=timer.ticks()*1000;
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Pre-update/ms"), t));
ULOGGER_DEBUG("time preUpdate=%f ms", t);
//============================================================
// Create a signature with the image received.
@@ -279,8 +281,9 @@ bool Memory::update(const SMState * smState, std::list<std::pair<std::string, fl
_lastLoopClosureId = signature->id();
}
stats.push_back(std::pair<std::string, float>(std::string("TimingMem/Signature creation/ms"), timer.ticks()*1000));
ULOGGER_DEBUG("time creating signature=%f ms", stats.back().second);
t=timer.ticks()*1000;
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Signature creation/ms"), t));
ULOGGER_DEBUG("time creating signature=%f ms", t);
//============================================================
// Comparison step...
@@ -295,8 +298,8 @@ bool Memory::update(const SMState * smState, std::list<std::pair<std::string, fl
UTimer t;
maxId = rehearsal(signature, _similarityOnlyLast, maxValue);
UDEBUG("t=%fs", t.ticks());
stats.push_back(std::pair<std::string, float>(std::string("Memory/Rehearsal Max Id/"), maxId));
stats.push_back(std::pair<std::string, float>(std::string("Memory/Rehearsal Value/"), maxValue));
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Max Id/"), maxId));
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Value/"), maxValue));
if(maxId > 0 && maxValue >= _similarityThreshold)
{
if(_incrementalMemory)
@@ -320,8 +323,10 @@ bool Memory::update(const SMState * smState, std::list<std::pair<std::string, fl
}
UDEBUG("t=%fs", t.ticks());
}
stats.push_back(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), timer.ticks()*1000));
ULOGGER_DEBUG("time rehearsal=%f ms", stats.back().second);
t=timer.ticks()*1000;
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
ULOGGER_DEBUG("time rehearsal=%f ms", t);
//============================================================
// Update the common signature
@@ -354,7 +359,7 @@ bool Memory::update(const SMState * smState, std::list<std::pair<std::string, fl
UDEBUG("totalTimer = %fs", totalTimer.ticks());
stats.push_back(std::pair<std::string, float>(std::string("Memory/Last loop closure/"), _lastLoopClosureId));
stats.insert(std::pair<std::string, float>(std::string("Memory/Last loop closure/"), _lastLoopClosureId));
return true;
}
@@ -630,7 +635,7 @@ void Memory::clear()
/**
* Compute the likelihood of the signature with some others in the memory. If
* the signature ids list is empty, the likelihood will be calculated
* for all signatures in the working time memory.
* for all signatures in the working memory.
* If an error occurs, the result is empty.
*/
std::map<int, float> Memory::computeLikelihood(const Signature * signature, const std::set<int> & signatureIds) const
@@ -655,21 +660,25 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
std::map<int, int>::const_iterator iter = wm.begin();
for(; iter!=wm.end(); ++iter)
{
float sim = signature->compareTo(this->getSignature(iter->first));
likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
sumSimilarity += sim;
UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
if(sim>maxSim)
//ignore the last in WM
if(iter->first != wm.rbegin()->first)
{
maxSim = sim;
}
if(sim)
{
++nonNulls;
}
else
{
++nulls;
float sim = signature->compareTo(this->getSignature(iter->first));
likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
sumSimilarity += sim;
UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
if(sim>maxSim)
{
maxSim = sim;
}
if(sim)
{
++nonNulls;
}
else
{
++nulls;
}
}
}
ULOGGER_DEBUG("sumSimilarity=%f, maxSim=%f, nonNulls=%d, nulls=%d)", sumSimilarity, maxSim, nonNulls, nulls);
@@ -829,13 +838,23 @@ Signature * Memory::getRemovableSignature(const std::list<int> & ignoredIds, boo
if(_lastLoopClosureId > 0 && _stMem.find(_lastLoopClosureId) == _stMem.end())
{
// If set, it must be in WM
bool nullWeightFound = false;
std::map<int, int>::const_iterator iter = _workingMem.find(_lastLoopClosureId);
while(iter != _workingMem.end())
{
++currentRecentWmSize;
Signature * s = uValue(_signatures, iter->first, (Signature*)0);
if(s && s->getWeight() == 0)
{
nullWeightFound = true;
}
else if(!s)
{
UERROR("Signature not found ?!?");
}
++iter;
}
if(currentRecentWmSize && currentRecentWmSize < recentWmMaxSize)
if(currentRecentWmSize>1 && !nullWeightFound && currentRecentWmSize < recentWmMaxSize)
{
recentWmImmunized = true;
}
@@ -1022,6 +1041,9 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
{
_lastLoopClosureId = newS->id();
}
// update weight
newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
}
else
{
@@ -1030,9 +1052,10 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
//this->merge(oldS, newS, kFullMerging);
_lastLoopClosureId = newS->id();
newS->setWeight(newS->getWeight() + oldS->getWeight());
}
newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
oldS->setLoopClosureId(newS->id());
// keep actions from the old

2
corelib/src/Memory.h
View File

@@ -53,7 +53,7 @@ public:
virtual ~Memory();
virtual void parseParameters(const ParametersMap & parameters);
bool update(const SMState * rawData, std::list<std::pair<std::string, float> > & stats);
bool update(const SMState * rawData, std::map<std::string, float> & stats);
virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
virtual int forget(const std::list<int> & ignoredIds = std::list<int>());

68
corelib/src/Rtabmap.cpp
View File

@@ -612,7 +612,7 @@ void Rtabmap::process()
double timeMemoryCleanup = 0;
double timeEmptyingMemoryTrash = 0;
double timeStatsCreation = 0;
std::list<std::pair<std::string, float> > memUpdateStats;
std::map<std::string, float> memUpdateStats;
int refId = Memory::kIdInvalid;
float hypothesisRatio = 0.0f; // Only used for statistics
@@ -781,7 +781,7 @@ void Rtabmap::process()
//============================================================
// Likelihood computation
// Get the likelihood of the new signature
// with all images contained in the working time memory + reactivated.
// with all images contained in the working memory + reactivated.
//============================================================
ULOGGER_INFO("computing likelihood...");
likelihood = _memory->computeLikelihood(signature);
@@ -854,26 +854,40 @@ void Rtabmap::process()
timeHypothesesValidation = timer.ticks();
ULOGGER_INFO("timeHypothesesValidation=%f",timeHypothesesValidation);
// We are tracking the next loop closures,
// reset the retrieval margin
_spreadMargin = 0;
}
//============================================================
// Retrieval id update
//============================================================
int lastReactivatedId = _reactivateId;
std::list<std::pair<int, float> > hyp;
this->selectHypotheses(posterior, hyp, false);
if(likelihood.at(hyp.front().first) > _remThr)
// using likelihood (only if in reactivated ids)
this->selectHypotheses(likelihood, hyp, false);
if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hyp.front().first) != reactivatedIds.end())
{
_reactivateId = hyp.front().first;
}
if(hypotheses.front().second < _loopRatio*_highestHypothesisValue)
else
{
if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hypotheses.front().first) != reactivatedIds.end())
//using highest hypothesis
this->selectHypotheses(posterior, hyp, false);
if(likelihood.at(hyp.front().first) > _remThr)
{
_reactivateId = hyp.front().first;
}
// else don't change the reactivateId
}
// Spreading margin...
if(hypotheses.front().second >= _loopThr && !rejectedHypothesis)
{
// We are tracking the next loop closures,
// reset the retrieval margin
_spreadMargin = 0;
UDEBUG("Margin=0");
}
else if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hypotheses.front().first) != reactivatedIds.end())
{
if(signaturesReactivated<2 || hypotheses.front().second < _loopRatio*_highestHypothesisValue)
{
// We are loosing the next loop closures (it
// can be temporary occlusions/bad images),
@@ -881,21 +895,9 @@ void Rtabmap::process()
++_spreadMargin;
UDEBUG("Margin++");
}
else
{
// We lost the next loop closures (a new path is taken),
// reset the retrieval margin
_spreadMargin = 0;
UDEBUG("Margin=0");
}
//else don't change the margin
}
else if(signaturesReactivated < 2 && _reactivateId == lastReactivatedId)
{
// If we are still, spread margin if no signatures were loaded...
++_spreadMargin;
UDEBUG("Margin++");
}
else if(signaturesReactivated >= 2)
else
{
// We lost the next loop closures (a new path is taken),
// reset the retrieval margin
@@ -982,6 +984,7 @@ void Rtabmap::process()
const KeypointSignature * ssRef = 0;
const KeypointSignature * ssLoop = 0;
int lcHypothesisReactivated = 0;
float rehearsalValue = uValue(memUpdateStats, std::string("Memory/Rehearsal Value/"), 0.0f);
KeypointMemory * kpMem = dynamic_cast<KeypointMemory *>(_memory);
if(kpMem)
{
@@ -1061,6 +1064,7 @@ void Rtabmap::process()
stat->addStatistic(Statistics::kLoopVp_likelihood(), vpLikelihood);
stat->addStatistic(Statistics::kLoopReactivateId(), _reactivateId);
stat->addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
stat->addStatistic(Statistics::kLoopRetrieval_margin(), _spreadMargin);
// Child count by parent signature on the root of the memory ... for statistics
stat->setWeights(_memory->getWeights());
@@ -1082,7 +1086,7 @@ void Rtabmap::process()
stat->addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
// memory update timings
for(std::list<std::pair<std::string, float> >::iterator iter = memUpdateStats.begin(); iter!=memUpdateStats.end(); ++iter)
for(std::map<std::string, float>::iterator iter = memUpdateStats.begin(); iter!=memUpdateStats.end(); ++iter)
{
stat->addStatistic(iter->first, iter->second);
}
@@ -1203,7 +1207,7 @@ void Rtabmap::process()
if(_foutFloat)
{
fprintf(_foutFloat, "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
fprintf(_foutFloat, "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
totalTime,
timeMemoryUpdate,
timeReactivations,
@@ -1220,12 +1224,13 @@ void Rtabmap::process()
mean,
stddev,
vpHypothesis,
timeEmptyingMemoryTrash);
timeEmptyingMemoryTrash,
rehearsalValue);
}
if(_foutInt)
{
fprintf(_foutInt, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %lu %d\n",
fprintf(_foutInt, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %lu %d %d\n",
_lcHypothesisId,
highestHypothesisId,
signaturesRemoved,
@@ -1241,7 +1246,8 @@ void Rtabmap::process()
refUniqueWordsCount,
_reactivateId,
nonNulls.size(),
directNeighborsNotReactivated);
directNeighborsNotReactivated,
_spreadMargin);
}
ULOGGER_INFO("Time logging = %f...", timer.ticks());
//ULogger::flush();

2
corelib/tests/Tests.cpp
View File

@@ -287,7 +287,7 @@ void Tests::testBayesFilter()
for(int i=1; i<11; ++i)
{
//ULOGGER_DEBUG("--- %d ---", i);
std::list<std::pair<std::string, float> > memStats;
std::map<std::string, float> memStats;
mem.update(0, memStats);
posterior = bayes.computePosterior(&mem, likelihood);
likelihood.insert(std::pair<int, float>(i, 1));