rtabmap: localization mode (run under fixed memory and fixed dictionary), optional retrieving of the last extracted words when calling rtabmap.process() (useful when in localization mode where the last location is deleted after the rtabmap process)

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

corelib/include/rtabmap/core/Memory.h

@@ -93,7 +93,8 @@ public:
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	float getSimilarityOnlyWithLast() const {return _rehearsalOnlyWithLast;}
-	const Signature * getLastSignature() const;
+	int getLastSignatureId() const;
+	const Signature * getLastWorkingSignature() const;
 	int getDatabaseMemoryUsed() const; // in bytes
 	double getDbSavingTime() const;
 	cv::Mat getImage(int signatureId) const;

corelib/include/rtabmap/core/Rtabmap.h

@@ -53,8 +53,8 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();
 
-	void process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
+	void process(const cv::Mat & image, int id=0, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience, an id is automatically generated if id=0
-	void process(const Image & image); // for convenience
+	void process(const Image & image, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience
 
 	void init(const ParametersMap & param, bool deleteMemory = true);
 	void init(const std::string & configFile = "", bool deleteMemory = true);

corelib/src/DBDriverSqlite3.cpp

@@ -822,8 +822,19 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			else
 			{
-				ULOGGER_ERROR("Node %d not found in database", *iter);
+				UWARN("Creating a empty signature for node %d", *iter);
-				rc = sqlite3_finalize(ppStmt);
+				Signature * ss = new Signature(*iter, visualWords);
+				if(ss)
+				{
+					ss->setWeight(weight);
+					ss->setSaved(true);
+					nodes.push_back(ss);
+				}
+				else
+				{
+					UFATAL("?Cannot allocate memory !!!");
+				}
+				++loaded;
 			}
 
 			//reset

corelib/src/Memory.cpp

@@ -35,7 +35,7 @@
 
 namespace rtabmap {
 
-const int Memory::kIdStart = 1;
+const int Memory::kIdStart = 0;
 const int Memory::kIdVirtual = -1;
 const int Memory::kIdInvalid = 0;
 
@@ -115,14 +115,22 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			_dbDriver->loadLastNodes(dbSignatures);
 			for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
 			{
-				_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
+				// ignore bad signatures
-				if((int)_stMem.size() <= _maxStMemSize)
+				if(!(*iter)->isBadSignature())
 				{
-					_stMem.insert((*iter)->id());
+					_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
+					if((int)_stMem.size() <= _maxStMemSize)
+					{
+						_stMem.insert((*iter)->id());
+					}
+					else
+					{
+						_workingMem.insert((*iter)->id());
+					}
 				}
 				else
 				{
-					_workingMem.insert((*iter)->id());
+					delete *iter;
 				}
 			}
 			UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
@@ -135,7 +143,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 			// Last id
 			_dbDriver->getLastNodeId(_idCount);
-			_idCount += 1;
 		}
 		else
 		{
@@ -149,7 +156,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 	_workingMem.insert(kIdVirtual);
 
-	ULOGGER_DEBUG("ids start with %d", _idCount);
+	ULOGGER_DEBUG("ids start with %d", _idCount+1);
 
 
 	// Now load the dictionary if we have a connection
@@ -810,7 +817,7 @@ void Memory::getLoopClosureIds(int signatureId, std::set<int> & loopClosureIds,
 
 int Memory::getNextId()
 {
-	return _idCount++;
+	return ++_idCount;
 }
 
 int Memory::getDatabaseMemoryUsed() const
@@ -1390,7 +1397,12 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
 	}
 }
 
-const Signature * Memory::getLastSignature() const
+int Memory::getLastSignatureId() const
+{
+	return _idCount;
+}
+
+const Signature * Memory::getLastWorkingSignature() const
 {
 	ULOGGER_DEBUG("");
 	return _lastSignature;
@@ -2229,6 +2241,10 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	{
 		id = this->getNextId();
 	}
+	else
+	{
+		_idCount = id;
+	}
 
 	int treeSize= _workingMem.size() + _stMem.size();
 	int nbCommonWords = 0;

corelib/src/Rtabmap.cpp

@@ -394,9 +394,9 @@ int Rtabmap::getRetrievedId() const
 int Rtabmap::getLastLocationId() const
 {
 	int id = 0;
-	if(_memory && _memory->getLastSignature())
+	if(_memory)
 	{
-		id = _memory->getLastSignature()->id();
+		id = _memory->getLastSignatureId();
 	}
 	return id;
 }
@@ -454,7 +454,7 @@ int Rtabmap::getTotalMemSize() const
 {
 	if(_memory)
 	{
-		const Signature * s  =_memory->getLastSignature();
+		const Signature * s  =_memory->getLastWorkingSignature();
 		if(s)
 		{
 			return s->id();
@@ -557,7 +557,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 //============================================================
 // MAIN LOOP
 //============================================================
-void Rtabmap::process(const Image & image)
+void Rtabmap::process(const Image & image, std::multimap<int, cv::KeyPoint> * words)
 {
 	UDEBUG("");
 
@@ -630,7 +630,7 @@ void Rtabmap::process(const Image & image)
 	{
 		return;
 	}
-	signature = _memory->getLastSignature();
+	signature = _memory->getLastWorkingSignature();
 	if(!signature)
 	{
 		UFATAL("Not supposed to be here...last signature is null?!?");
@@ -640,6 +640,11 @@ void Rtabmap::process(const Image & image)
 	timeMemoryUpdate = timer.ticks();
 	ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
 
+	if(words)
+	{
+		*words = signature->getWords();
+	}
+
 	//============================================================
 	// Bayes filter update
 	//============================================================
@@ -1229,10 +1234,10 @@ void Rtabmap::rejectLastLoopClosure()
 	}
 }
 
-void Rtabmap::process(const cv::Mat & image, int id)
+void Rtabmap::process(const cv::Mat & image, int id, std::multimap<int, cv::KeyPoint> * words)
 {
 
-	this->process(Image(image, id));
+	this->process(Image(image, id), words);
 }
 
 void Rtabmap::dumpData() const

guilib/src/DatabaseViewer.cpp

@@ -315,9 +315,9 @@ void DatabaseViewer::update(int value,
 				img.loadFromData(iter.value(), "BMP");
 			}
 
-			if(memory_ && dynamic_cast<rtabmap::Memory*>(memory_))
+			if(memory_)
 			{
-				std::multimap<int, cv::KeyPoint> words = dynamic_cast<rtabmap::Memory*>(memory_)->getWords(id);
+				std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
 				if(words.size())
 				{
 					drawKeypoints(words, view->scene());