rtabmap: added rejectLastLoopClosure() method, for convenience if a validation is done outside RTAB-Map and to reject loop closure detected by RTAB-Map.

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@782 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Memory.h

@@ -76,6 +76,7 @@ public:
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
 	void deleteLastLocation();
+	void rejectLastLoopClosure();
 
 	//getters
 	const std::set<int> & getWorkingMem() const {return _workingMem;}
@@ -172,6 +173,7 @@ private:
 	int _lastLoopClosureId;
 	bool _memoryChanged; // False by default, become true when Memory::update() is called.
 	int _signaturesAdded;
+	std::vector<std::pair<int, int> > _savedLoopClosureInfo; // size 3 or 0
 
 	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

corelib/include/rtabmap/core/Rtabmap.h

@@ -86,7 +86,7 @@ public:
 	int getLoopClosureId() const;
 	int getRetrievedId() const;
 	int getLastLocationId();
-	float getLcHypValue() const {return _lastLcHypothesisValue;}
+	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM(); // working memory
 	std::set<int> getSTM(); // short-term memory
 	int getWMSize(); // working memory size
@@ -108,6 +108,7 @@ public:
 	void updateParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
 	void deleteLastLocation();
+	void rejectLastLoopClosure();
 
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
@@ -148,9 +149,8 @@ private:
 	bool _statisticLogsBufferedInRAM;
 
 	int _lcHypothesisId;
+	float _lcHypothesisValue;
 	int _retrievedId;
-	float _lastLcHypothesisValue;
-	int _lastLoopClosureId;
 	double _lastProcessTime;
 
 	UMutex _stateMutex;

corelib/src/Memory.cpp

@@ -1391,7 +1391,7 @@ void Memory::deleteLastLocation()
 	{
 		UDEBUG("deleting last location %d", lastSignature->id());
 		const std::set<int> & neighbors = lastSignature->getNeighbors();
-		for(std::set<int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+		for(std::set<int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
 		{
 			Signature * s = _getSignature(*iter);
 			if(s)
@@ -1402,7 +1402,7 @@ void Memory::deleteLastLocation()
 		}
 
 		const std::set<int> & child = lastSignature->getChildLoopClosureIds();
-		for(std::set<int>::iterator iter=child.begin(); iter!=child.end(); ++iter)
+		for(std::set<int>::const_iterator iter=child.begin(); iter!=child.end(); ++iter)
 		{
 			Signature * s = _getSignature(*iter);
 			if(s)
@@ -1411,11 +1411,47 @@ void Memory::deleteLastLocation()
 				s->setWeight(s->getWeight() + lastSignature->getWeight());
 			}
 		}
+		lastSignature->setWeight(0);
 
 		this->moveToTrash(lastSignature);
 	}
 }
 
+void Memory::rejectLastLoopClosure()
+{
+	if(_lastSignature && _savedLoopClosureInfo.size())
+	{
+		UDEBUG("removing loop closure from location %d", _lastSignature->id());
+		UASSERT(_savedLoopClosureInfo.size() == 3);
+		// format _savedLoopClosureInfo[]
+		// 0-previous last loop closure id
+		// 1-parent loop closure id + weight
+		// 2-child loop closure id + weight
+
+		if(_savedLoopClosureInfo[1].first == _lastSignature->id())
+		{
+			std::set<int> child = _lastSignature->getChildLoopClosureIds();
+			for(std::set<int>::iterator iter=child.begin(); iter!=child.end(); ++iter)
+			{
+				if(*iter == _savedLoopClosureInfo[2].first)
+				{
+					Signature * s = _getSignature(*iter);
+					if(s)
+					{
+						s->removeLoopClosureId(_lastSignature->id());
+						s->setWeight(_savedLoopClosureInfo[2].second);
+					}
+					_lastSignature->removeChildLoopClosureId(*iter);
+					_lastSignature->setWeight(_savedLoopClosureInfo[1].second);
+					break;
+				}
+			}
+			_lastLoopClosureId = _savedLoopClosureInfo.begin()->first;
+		}
+		_savedLoopClosureInfo.clear();
+	}
+}
+
 bool Memory::addLoopClosureLink(int oldId, int newId)
 {
 	ULOGGER_INFO("old=%d, new=%d", oldId, newId);
@@ -1439,12 +1475,25 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 			// During loop closure in WM
 			oldS->addLoopClosureId(newS->id());
 			newS->addChildLoopClosureId(oldS->id());
+
+			//save stat to be used if it is rejected after (see rejectLastLoopClosure())
+			_savedLoopClosureInfo.clear();
+			_savedLoopClosureInfo.push_back(std::pair<int, int>(_lastLoopClosureId, 0));
+			_savedLoopClosureInfo.push_back(std::pair<int, int>(newS->id(), newS->getWeight()));
+			_savedLoopClosureInfo.push_back(std::pair<int, int>(oldS->id(), oldS->getWeight()));
+
 			_lastLoopClosureId = newS->id();
 			newS->setWeight(newS->getWeight() + oldS->getWeight());
 			oldS->setWeight(0);
 			return true; // RETURN
 		}
 
+		//update loop closure info if the merged location had a loop closure...
+		if(_savedLoopClosureInfo.size() == 3 && _savedLoopClosureInfo[1].first == oldS->id())
+		{
+			_savedLoopClosureInfo.clear();
+		}
+
 		// During rehearsal in STM
 		if(_idUpdatedToNewOneRehearsal)
 		{

corelib/src/Rtabmap.cpp

@@ -71,9 +71,8 @@ Rtabmap::Rtabmap() :
 	_likelihoodNullValuesIgnored(Parameters::defaultRtabmapLikelihoodNullValuesIgnored()),
 	_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
 	_lcHypothesisId(0),
+	_lcHypothesisValue(0),
 	_retrievedId(0),
-	_lastLcHypothesisValue(0),
-	_lastLoopClosureId(0),
 	_lastProcessTime(0.0),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
@@ -665,7 +664,9 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 	}
 	this->clearBufferedSensors();
 	_retrievedId = 0;
-	_lastLcHypothesisValue = 0;
+	_lcHypothesisValue = 0;
+	_lcHypothesisId = 0;
+	_lastProcessTime = 0.0;
 	this->setupLogFiles(dbOverwritten);
 }
 
@@ -917,7 +918,7 @@ void Rtabmap::process()
 					// information (like the epipolar geometry or using the local map
 					// associated with the signature)
 					//============================================================
-					if(_lastLcHypothesisValue && hypothesis.second >= _loopRatio*_lastLcHypothesisValue &&
+					if(_lcHypothesisValue && hypothesis.second >= _loopRatio*_lcHypothesisValue &&
 					   (!_epipolarGeometry || _epipolarGeometry->check(signature, _memory->getSignature(hypothesis.first))))
 					{
 						_lcHypothesisId = hypothesis.first;
@@ -930,7 +931,7 @@ void Rtabmap::process()
 					timeHypothesesValidation = timer.ticks();
 					ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
 				}
-				else if(hypothesis.second < _loopRatio*_lastLcHypothesisValue)
+				else if(hypothesis.second < _loopRatio*_lcHypothesisValue)
 				{
 					// Used for Precision-Recall computation.
 					// When analysing logs, it's convenient to know
@@ -944,8 +945,8 @@ void Rtabmap::process()
 				_retrievedId = hypothesis.first;
 
 				//for statistic...
-				hypothesisRatio = _lastLcHypothesisValue>0?hypothesis.second/_lastLcHypothesisValue:0;
-				_lastLcHypothesisValue = hypothesis.second;
+				hypothesisRatio = _lcHypothesisValue>0?hypothesis.second/_lcHypothesisValue:0;
+				_lcHypothesisValue = hypothesis.second;
 
 			}
 		} // if(_memory->getWorkingMemSize())
@@ -1338,7 +1339,7 @@ void Rtabmap::process()
 								timeHypothesesValidation,
 								timeRealTimeLimitReachedProcess,
 								timeStatsCreation,
-								_lastLcHypothesisValue,
+								_lcHypothesisValue,
 								0.0f,
 								maxLikelihood,
 								sumLikelihoods,
@@ -1508,6 +1509,25 @@ void Rtabmap::deleteLastLocation()
 	}
 }
 
+void Rtabmap::rejectLastLoopClosure()
+{
+	UScopeMutex s(&_threadMutex);
+	UDEBUG("_lcHypothesisId=%d", _lcHypothesisId);
+	if(_lcHypothesisId)
+	{
+		_lcHypothesisId = 0;
+		if(_memory)
+		{
+			_memory->rejectLastLoopClosure();
+		}
+		if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
+		{
+			statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
+		}
+		statistics_.setLoopClosureId(0);
+	}
+}
+
 void Rtabmap::process(const cv::Mat & image, int id)
 {
 	UScopeMutex s(&_threadMutex);

corelib/src/RtabmapEvent.cpp

@@ -18,6 +18,7 @@
  */
 
 #include "rtabmap/core/RtabmapEvent.h"
+#include <utilite/UStl.h>
 
 namespace rtabmap {
 std::map<std::string, float> Statistics::_defaultData;
@@ -44,7 +45,7 @@ Statistics::~Statistics()
 // name format = "Grp/Name/unit"
 void Statistics::addStatistic(const std::string & name, float value)
 {
-	_data.insert(std::pair<std::string, float>(name, value));
+	uInsert(_data, std::pair<std::string, float>(name, value));
 }
 
 void Statistics::setRefImage(const cv::Mat & image)