rtabmap: added parameter to keep the old id or to update to new id when merging on rehearsal

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

corelib/include/rtabmap/core/Memory.h

@@ -163,7 +163,8 @@ private:
 	bool _incrementalMemory;
 	int _maxStMemSize;
 	float _recentWmRatio;
-	bool _dataMergedOnRehearsal;
+	bool _oldDataKeptOnRehearsal;
+	bool _idUpdatedToNewOneRehearsal;
 
 	int _idCount;
 	Signature * _lastSignature;

corelib/include/rtabmap/core/Parameters.h

@@ -144,7 +144,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 30); // Short-term memory size
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true);
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
-	RTABMAP_PARAM(Mem, DataMergedOnRehearsal,   bool, true); // Merge data on rehearsal
+	RTABMAP_PARAM(Mem, RehearsalOldDataKept,    bool, true); // On merge, keep old data
+	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,   bool, true); // On merge, update to new id
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true); // Publishing keypoints

corelib/include/rtabmap/core/Rtabmap.h

@@ -94,8 +94,9 @@ public:
 	std::map<int, int> getWeights();
 	int getTotalMemSize();
 	double getLastProcessTime() const {return _lastProcessTime;};
-	std::multimap<int, cv::KeyPoint> getWords(int nodeId);
+	std::multimap<int, cv::KeyPoint> getWords(int locationId);
 	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false);
+	bool isInSTM(int locationId);
 
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 

corelib/src/DBDriverSqlite3.cpp

@@ -1106,6 +1106,7 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::set<int> & neighb
 
 		query << "SELECT to_id FROM Link "
 		      << "WHERE from_id = " << signatureId
+		      << " AND type = 0"
 			  << " ORDER BY to_id";
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -1133,7 +1134,7 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::set<int> & neighb
 
 		if(neighbors.size() == 0)
 		{
-			UERROR("No neighbors loaded from signature %d", signatureId);
+			//UERROR("No neighbors loaded from signature %d", signatureId);
 		}
 	}
 }

corelib/src/Memory.cpp

@@ -47,7 +47,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
 	_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
-	_dataMergedOnRehearsal(Parameters::defaultMemDataMergedOnRehearsal()),
+	_oldDataKeptOnRehearsal(Parameters::defaultMemRehearsalOldDataKept()),
+	_idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
 	_idCount(kIdStart),
 	_lastSignature(0),
 	_lastLoopClosureId(0),
@@ -259,15 +260,26 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	{
 		this->setRecentWmRatio(std::atof((*iter).second.c_str()));
 	}
-	if((iter=parameters.find(Parameters::kMemDataMergedOnRehearsal())) != parameters.end())
+	if((iter=parameters.find(Parameters::kMemRehearsalOldDataKept())) != parameters.end())
 	{
-		_dataMergedOnRehearsal = uStr2Bool((*iter).second.c_str());
+		_oldDataKeptOnRehearsal = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kMemRehearsalIdUpdatedToNewOne())) != parameters.end())
+	{
+		_idUpdatedToNewOneRehearsal = uStr2Bool((*iter).second.c_str());
 	}
 	if(_dbDriver)
 	{
 		_dbDriver->parseParameters(parameters);
 	}
 
+	// verification...
+	if(!_idUpdatedToNewOneRehearsal && !_rehearsalOnlyWithLast)
+	{
+		UWARN("if _idUpdatedToNewOneRehearsal=false, _rehearsalOnlyWithLast must be true");
+		_rehearsalOnlyWithLast = true;
+	}
+
 
 	// Keypoint stuff
 	if(_vwd)
@@ -1348,13 +1360,10 @@ void Memory::moveToTrash(Signature * s)
 				// neighbor to s
 				if(n)
 				{
-					std::set<int>::const_iterator jter = n->getNeighbors().find(s->id());
-					if(jter != n->getNeighbors().end())
-					{
-						n->removeNeighbor(s->id());
-					}
+					n->removeNeighbor(s->id());
 				}
 			}
+			s->removeNeighbors();
 		}
 
 		if(	_dbDriver &&
@@ -1390,85 +1399,77 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 			return true;
 		}
 
-		// Set loop closure link
-		oldS->addLoopClosureId(newS->id());
 		UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id());
 
 		bool onRehearsal = this->isInSTM(oldS->id());
 		if(!onRehearsal)
 		{
 			// During loop closure in WM
+			oldS->addLoopClosureId(newS->id());
 			newS->addChildLoopClosureId(oldS->id());
 			_lastLoopClosureId = newS->id();
 			newS->setWeight(newS->getWeight() + oldS->getWeight());
 			oldS->setWeight(0);
-			return true;
+			return true; // RETURN
 		}
-		else
+
+		// During rehearsal in STM
+		if(_idUpdatedToNewOneRehearsal)
 		{
-			UDEBUG("On rehearsal");
-			// During rehearsal in STM...
-			// Here we merge the new location with the old one,
-			// redirecting all neighbor links to new location.
+			// update weight
+			newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
+
+			oldS->addLoopClosureId(newS->id()); // to keep track of the merged location
+
 			if(_lastLoopClosureId == oldS->id())
 			{
 				_lastLoopClosureId = newS->id();
 			}
-
+		}
+		else
+		{
 			// update weight
-			newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
+			oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
 
-			// redirect all baseIds from old id to new id
-			for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
+			newS->addLoopClosureId(oldS->id()); // to keep track of the merged location
+
+			if(_lastSignature == newS)
 			{
-				Signature * s = _getSignature(*iter);
-				if(s)
-				{
-					s->changeNeighborIds(oldS->id(), newS->id());
-				}
-				else if(!s)
-				{
-					UERROR("Location %d is not in RAM?!?", *iter);
-				}
+				_lastSignature = oldS;
 			}
 		}
 
-		UDEBUG("");
-		// redirect neighbor links
-		std::set<int> neighbors = oldS->getNeighbors();
-		for(std::set<int>::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
+		if(_idUpdatedToNewOneRehearsal)
 		{
-			int link = *iter;
-			if(link != newS->id() && link != oldS->id())
+			// redirect neighbor links
+			std::set<int> neighbors = oldS->getNeighbors();
+			for(std::set<int>::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
 			{
-				Signature * s = this->_getSignature(link);
-				if(s)
+				int link = *iter;
+				if(link != newS->id() && link != oldS->id())
 				{
-					// modify neighbor "from"
-					s->changeNeighborIds(oldS->id(), newS->id());
-
-					if(!newS->hasNeighbor(link))
+					Signature * s = this->_getSignature(link);
+					if(s)
 					{
-						newS->addNeighbor(link);
+						// modify neighbor "from"
+						s->changeNeighborIds(oldS->id(), newS->id());
+
+						if(!newS->hasNeighbor(link))
+						{
+							newS->addNeighbor(link);
+						}
+					}
+					else
+					{
+						UERROR("Didn't find neighbor %d of %d in RAM...", link, oldS->id());
 					}
 				}
-				else
-				{
-					UERROR("Didn't find neighbor %d of %d in RAM...", link, oldS->id());
-				}
 			}
+			oldS->removeNeighbors();
 
-			if(link == newS->id())
-			{
-				oldS->removeNeighbor(link);
-			}
-		}
-
-		if(onRehearsal)
-		{
 			// redirect child loop closure links
-			const std::set<int> & childIds = oldS->getChildLoopClosureIds();
-			for(std::set<int>::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
+			std::set<int> childIds = oldS->getChildLoopClosureIds();
+			for(std::set<int>::iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
 			{
 				if(*iter == newS->id())
 				{
@@ -1486,18 +1487,27 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 				{
 					UERROR("A location (%d, child of %d) in WM/STM cannot be transferred if its loop closure id is in STM", *iter, oldS->id());
 				}
+				oldS->removeChildLoopClosureId(*iter);
 			}
-
-			if(_dataMergedOnRehearsal)
-			{
-				this->copyData(oldS, newS);
-				// Set old image to new signature
-			}
-
-			// remove old location
-			moveToTrash(oldS);
 		}
 
+		//remove mutual links
+		oldS->removeNeighbor(newId);
+		newS->removeNeighbor(oldId);
+
+		if(_oldDataKeptOnRehearsal && _idUpdatedToNewOneRehearsal)
+		{
+			// Set old image to new signature
+			this->copyData(oldS, newS);
+		}
+		else if(!_oldDataKeptOnRehearsal && !_idUpdatedToNewOneRehearsal)
+		{
+			this->copyData(newS, oldS);
+		}
+
+		// remove location
+		moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS);
+
 		return true;
 	}
 	else
@@ -1755,7 +1765,14 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 					 if(id!=*iter)
 					 {
 						 int weightNeighbor = 0;
-						 _dbDriver->getWeight(*iter, weightNeighbor);
+						 if(_signatures.find(*iter) == _signatures.end())
+						 {
+							 _dbDriver->getWeight(*iter, weightNeighbor);
+						 }
+						 else
+						 {
+							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
+						 }
 						 UDEBUG("Add neighbor link from %d to %d", id, *iter);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
 								 id,
@@ -1777,6 +1794,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 					 {
 						 weightNeighbor = _signatures.find(*iter)->second->getWeight();
 					 }
+					 UDEBUG("Add loop link from %d to %d", id, *iter);
 					 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
 							 id,
 							 weight,
@@ -1795,6 +1813,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 					 {
 						 weightNeighbor = _signatures.find(*iter)->second->getWeight();
 					 }
+					 UDEBUG("Add child link from %d to %d", id, *iter);
 					 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
 							 id,
 							 weight,
@@ -1850,6 +1869,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 						 {
 							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
 						 }
+						 UDEBUG("Add loop link from %d to %d", id, *iter);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
 								 id,
 								 weight,
@@ -1871,6 +1891,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 						 {
 							 weightNeighbor = _signatures.find(*iter)->second->getWeight();
 						 }
+						 UDEBUG("Add child link from %d to %d", id, *iter);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
 								 id,
 								 weight,

corelib/src/Rtabmap.cpp

@@ -468,12 +468,11 @@ std::map<int, int> Rtabmap::getWeights()
 std::set<int> Rtabmap::getSTM()
 {
 	UScopeMutex s(&_threadMutex);
-	std::set<int> mem;
 	if(_memory)
 	{
-		mem = _memory->getStMem();
+		return _memory->getStMem();
 	}
-	return mem;
+	return std::set<int>();
 }
 
 int Rtabmap::getSTMSize()
@@ -489,47 +488,49 @@ int Rtabmap::getSTMSize()
 int Rtabmap::getTotalMemSize()
 {
 	UScopeMutex s(&_threadMutex);
-	ULOGGER_DEBUG("");
-	int memSize = 0;
 	if(_memory)
 	{
 		const Signature * s  =_memory->getLastSignature();
 		if(s)
 		{
-			memSize = s->id();
+			return s->id();
 		}
 	}
-	return memSize;
+	return 0;
 }
 
-std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int nodeId)
+std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId)
 {
 	UScopeMutex s(&_threadMutex);
-	std::multimap<int, cv::KeyPoint> words;
-
 	if(_memory)
 	{
-		const Signature * s = _memory->getSignature(nodeId);
+		const Signature * s = _memory->getSignature(locationId);
 		if(s)
 		{
-			words = s->getWords();
+			return s->getWords();
 		}
 	}
-
-	return words;
+	return std::multimap<int, cv::KeyPoint>();
 }
 
 std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM)
 {
 	UScopeMutex s(&_threadMutex);
-	std::map<int, int> ids;
-
 	if(_memory)
 	{
-		ids = _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
+		return _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
 	}
+	return std::map<int, int>();
+}
 
-	return ids;
+bool Rtabmap::isInSTM(int locationId)
+{
+	UScopeMutex s(&_threadMutex);
+	if(_memory)
+	{
+		return _memory->isInSTM(locationId);
+	}
+	return false;
 }
 
 void Rtabmap::clearBufferedSensors()

corelib/src/Signature.h

@@ -59,6 +59,7 @@ public:
 	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
 	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
 	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
+	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
 	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
 	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
 	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}