Added statistics : Rehearsal value, retrieval margin

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

Matlab/ShowLogs/showlogs.m

@@ -47,6 +47,7 @@ LogF = importfile([PathPrefix '/' 'LogF.txt']);
 % 15 stddev likelihood
 % 16 vp hypothesis
 % 17 timeEmtyingTrash
+% 18 rehearsalValue
 
 LogI = importfile([PathPrefix '/' 'LogI.txt']);
 % COLUMN HEADERS : 
@@ -63,8 +64,10 @@ LogI = importfile([PathPrefix '/' 'LogI.txt']);
 % 11 signaturesReactivated
 % 12 lcHypothesisReactivated
 % 13 refUniqueWordsCount
-% 14 reactivateId
+% 14 _reactivateId
-% 15 non nulls count
+% 15 nonNulls.size()
+% 16 directNeighborsNotReactivated
+% 17 _spreadMargin
 
 if isempty(LogI) || isempty(LogF)
     error('Log files are empty')

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -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,);

corelib/src/KeypointMemory.cpp

@@ -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));
+				//ignore the last in WM
-				if(_tfIdfNormalized)
+				if(iter->first != wm.rbegin()->first)
 				{
-					const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
+					likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
-					float wordsCountRatio = -1; // default invalid
+					if(_tfIdfNormalized)
-					if(s)
 					{
-						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));
 					}
 				}
 			}

corelib/src/Memory.cpp

@@ -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));
+	t=timer.ticks()*1000;
-	ULOGGER_DEBUG("time preUpdate=%f ms", stats.back().second);
+	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));
+	t=timer.ticks()*1000;
-	ULOGGER_DEBUG("time creating signature=%f ms", stats.back().second);
+	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.insert(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 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));
+			//ignore the last in WM
-			likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
+			if(iter->first != wm.rbegin()->first)
-			sumSimilarity += sim;
-			UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
-			if(sim>maxSim)
 			{
-				maxSim = sim;
+				float sim = signature->compareTo(this->getSignature(iter->first));
-			}
+				likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
-			if(sim)
+				sumSimilarity += sim;
-			{
+				UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
-				++nonNulls;
+				if(sim>maxSim)
-			}
+				{
-			else
+					maxSim = sim;
-			{
+				}
-				++nulls;
+				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

corelib/src/Memory.h

@@ -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>());

corelib/src/Rtabmap.cpp

@@ -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);
+				// using likelihood (only if in reactivated ids)
-				if(likelihood.at(hyp.front().first) > _remThr)
+				this->selectHypotheses(likelihood, hyp, false);
+				if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hyp.front().first) != reactivatedIds.end())
 				{
 					_reactivateId = hyp.front().first;
 				}
-
+				else
-				if(hypotheses.front().second < _loopRatio*_highestHypothesisValue)
 				{
-					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
+					//else don't change the margin
-					{
-						// We lost the next loop closures (a new path is taken),
-						// reset the retrieval margin
-						_spreadMargin = 0;
-						UDEBUG("Margin=0");
-					}
 				}
-				else if(signaturesReactivated < 2 && _reactivateId == lastReactivatedId)
+				else
-				{
-					// If we are still, spread margin if no signatures were loaded...
-					++_spreadMargin;
-					UDEBUG("Margin++");
-				}
-				else if(signaturesReactivated >= 2)
 				{
 					// 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();

corelib/tests/Tests.cpp

@@ -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));

guilib/src/MainWindow.cpp

@@ -888,7 +888,7 @@ void MainWindow::captureScreen()
 		dir.mkdir(targetDir);
 	}
 	targetDir += "/";
-	QString name = QDateTime::currentDateTime().toString("yyMMdd-hhmmsszzz") + ".png";
+	QString name = QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png";
 	_ui->statusbar->clearMessage();
 	QPixmap figure = QPixmap::grabWidget(this);
 	figure.save(targetDir + name);

guilib/src/Plot.cpp

@@ -2047,7 +2047,7 @@ void Plot::captureScreen()
 		dir.mkdir(targetDir);
 	}
 	targetDir += "/";
-	QString name = QDateTime::currentDateTime().toString("yyMMdd-hhmmsszzz") + ".png";
+	QString name = QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png";
 	QPixmap figure = QPixmap::grabWidget(this);
 	figure.save(targetDir + name);
 }