Fixed saving/loading dictionary (not loaded words)

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

corelib/include/rtabmap/core/DBDriver.h

@@ -112,9 +112,11 @@ private:
 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
 	virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;
 
-	virtual void saveQuery(const std::vector<VisualWord *> & visualWords) const = 0;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
+
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
@@ -131,6 +133,7 @@ private:
 private:
 	//non-abstract methods
 	void saveOrUpdate(const std::vector<Signature *> & signatures) const;
+	void saveOrUpdate(const std::vector<VisualWord *> & words) const;
 
 	//thread stuff
 	virtual void mainLoop();

corelib/src/DBDriver.cpp

@@ -178,7 +178,7 @@ void DBDriver::emptyTrashes(bool async)
 			if(this->isConnected())
 			{
 				//Only one query to the database
-				this->saveQuery(uValues(visualWords));
+				this->saveOrUpdate(uValues(visualWords));
 			}
 
 			for(std::map<int, VisualWord *>::iterator iter=visualWords.begin(); iter!=visualWords.end(); ++iter)
@@ -223,7 +223,6 @@ void DBDriver::asyncSave(VisualWord * vw)
 	}
 }
 
-//Automatically begin and commit a transaction
 void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 {
 	ULOGGER_DEBUG("");
@@ -254,6 +253,36 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 	}
 }
 
+void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
+{
+	ULOGGER_DEBUG("");
+	std::list<VisualWord *> toSave;
+	std::list<VisualWord *> toUpdate;
+	if(this->isConnected() && words.size())
+	{
+		for(std::vector<VisualWord *>::const_iterator i=words.begin(); i!=words.end();++i)
+		{
+			if((*i)->isSaved())
+			{
+				toUpdate.push_back(*i);
+			}
+			else
+			{
+				toSave.push_back(*i);
+			}
+		}
+
+		if(toUpdate.size())
+		{
+			this->updateQuery(toUpdate);
+		}
+		if(toSave.size())
+		{
+			this->saveQuery(toSave);
+		}
+	}
+}
+
 void DBDriver::load(VWDictionary * dictionary) const
 {
 	_dbSafeAccessMutex.lock();

corelib/src/DBDriverSqlite3.cpp

@@ -859,16 +859,16 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 		timer.start();
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
+		std::string query;
 		std::list<int> ids;
 
 		// Get the map from signature and visual words
-		query << "SELECT n.id "
+		query = "SELECT n.id "
 				 "FROM Node AS n "
 				 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
 				 "ORDER BY n.id;";
 
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 		// Process the result if one
@@ -891,7 +891,6 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 	}
 }
 
-// TODO DO IT IN A MORE EFFICiENT WAY !!!!
 void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 {
 	ULOGGER_DEBUG("");
@@ -902,85 +901,111 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 		timer.start();
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
+		std::string query;
 		std::list<VisualWord *> visualWords;
 
-		// Get the map from signature and visual words
+		// Get the visual words
-		query << "SELECT vw.id, vw.descriptor_size, vw.descriptor, m.node_id "
+		query = "SELECT id, descriptor_size, descriptor "
-				 "FROM Word as vw "
+				"FROM Word "
-				 "INNER JOIN Map_Node_Word as m "
+				"WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
-				 "ON vw.id=m.word_id "
+				"ORDER BY id;";
-				 "WHERE vw.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
-				 "ORDER BY vw.id;";
 
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 		// Process the result if one
 		int id = 0;
-		int lastId = 0;
 		int descriptorSize = 0;
 		const void * descriptor = 0;
 		int dRealSize = 0;
-		int signatureId;
 		rc = sqlite3_step(ppStmt);
 		int count = 0;
 		while(rc == SQLITE_ROW)
 		{
-			id = sqlite3_column_int(ppStmt, 0); 			// VisualWord Id
+			int index=0;
+			id = sqlite3_column_int(ppStmt, index++); 			// VisualWord Id
 			if(id>0)
 			{
-				if(id != lastId)
+				descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-				{
+				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-					descriptorSize = sqlite3_column_int(ppStmt, 1); // VisualWord descriptor size
+				dRealSize = sqlite3_column_bytes(ppStmt, index++);
-					descriptor = sqlite3_column_blob(ppStmt, 2); 	// VisualWord descriptor array
+
-					dRealSize = sqlite3_column_bytes(ppStmt, 2);
-				}
 				if(dRealSize/int(sizeof(float)) != descriptorSize)
 				{
 					UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
 				}
-				lastId = id;
+				VisualWord * vw = new VisualWord(id, &((const float *)descriptor)[0], descriptorSize, 0);
-				signatureId = sqlite3_column_int(ppStmt, 3); 	// Signature ref Id
-				if(dictionary->getWord(id) != 0)
-				{
-					// Use VWDictionary::addWordRef instead of VisualWord::addRef()
-					// This will increment _totalActiveReferences in the dictionary
-					dictionary->addWordRef(id, signatureId);
-				}
-				else
-				{
-					VisualWord * vw = new VisualWord(id, &((const float *)descriptor)[0], descriptorSize, signatureId);
-					if(vw)
-					{
 				vw->setSaved(true);
 				dictionary->addWord(vw);
 			}
 			else
-					{
-						ULOGGER_ERROR("Couldn't create a Visual word!?");
-					}
-				}
-			}
-			else
 			{
 				ULOGGER_ERROR("Wrong word id ?!? (%d)", id);
 			}
 			if(++count % 5000 == 0)
+			{
+				ULOGGER_DEBUG("Loaded %d words...", count);
+			}
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		/*
+		// Load references
+		// Get the map from signature and visual words
+		query = "SELECT node_id "
+				"FROM Map_Node_Word "
+				"INNER JOIN Node "
+				"ON Node.id = node_id "
+				"WHERE word_id = ? AND node.time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		int signatureId;
+		count = 0;
+		for(std::map<int, VisualWord*>::const_iterator iter=dictionary->getVisualWords().begin(); iter!=dictionary->getVisualWords().end(); ++iter)
+		{
+			// bind id
+			rc = sqlite3_bind_int(ppStmt, 1, iter->first);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+			rc = sqlite3_step(ppStmt);
+			while(rc == SQLITE_ROW)
+			{
+				int index=0;
+				signatureId = sqlite3_column_int(ppStmt, index++);
+				if(signatureId>0)
+				{
+					dictionary->addWordRef(iter->first, signatureId);
+				}
+				else
+				{
+					ULOGGER_ERROR("Wrong signature id ?!? (%d)", signatureId);
+				}
+				if(++count % 5000 == 0)
 				{
 					ULOGGER_DEBUG("Loaded %d word references...", count);
 				}
 				rc = sqlite3_step(ppStmt); // next result...
 			}
-
-		getLastWordId(id);
-		dictionary->setLastWordId(id);
-
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+			//reset
+			rc = sqlite3_reset(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());*/
+
+		// Get Last word id
+		getLastWordId(id);
+		dictionary->setLastWordId(id);
 
 		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 	}
@@ -1308,6 +1333,45 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 	}
 }
 
+void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words) const
+{
+	if(_ppDb && words.size())
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		VisualWord * w = 0;
+
+		std::string query = "UPDATE Word SET time_enter = DATETIME('NOW') WHERE id=?;";
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		for(std::list<VisualWord *>::const_iterator i=words.begin(); i!=words.end(); ++i)
+		{
+			w = *i;
+			int index = 1;
+			if(w)
+			{
+				rc = sqlite3_bind_int(ppStmt, index++, w->id());
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+				//step
+				rc=sqlite3_step(ppStmt);
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+				rc = sqlite3_reset(ppStmt);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			}
+		}
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		ULOGGER_DEBUG("Update Word table, Time=%fs", timer.ticks());
+	}
+}
+
 void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 {
 	UDEBUG("");
@@ -1400,7 +1464,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 	}
 }
 
-void DBDriverSqlite3::saveQuery(const std::vector<VisualWord *> & words) const
+void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 {
 	UDEBUG("visualWords size=%d", words.size());
 	if(_ppDb)
@@ -1418,7 +1482,7 @@ void DBDriverSqlite3::saveQuery(const std::vector<VisualWord *> & words) const
 			query = std::string("INSERT INTO Word(id, descriptor_size, descriptor) VALUES(?,?,?);");
 			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-			for(std::vector<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+			for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				const VisualWord * w = *iter;
 				if(w && !w->isSaved())

corelib/src/DBDriverSqlite3.h

@@ -51,9 +51,10 @@ private:
 	virtual void getWeightQuery(int signatureId, int & weight) const;
 	virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
 
-	virtual void saveQuery(const std::vector<VisualWord *> & visualWords) const;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const;
 	virtual void saveQuery(const std::list<Signature *> & signatures) const;
+	virtual void saveQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<Signature *> & signatures) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;

corelib/src/Memory.cpp

@@ -139,14 +139,15 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 
 	// Now load the dictionary if we have a connection
-	if(_vwd && _dbDriver && _dbDriver->isConnected())
+	if(_dbDriver && _dbDriver->isConnected())
 	{
 		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary...")));
 		_dbDriver->load(_vwd);
-		UDEBUG("%d words loaded!", _vwd->getVisualWords().size());
+		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
-		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2Str(int(_vwd->getVisualWords().size())) + " loaded)"));
+		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2Str(int(_vwd->getUnusedWordsSize())) + " words)"));
 	}
 
+	UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
 	// Enable loaded signatures
 	Signature * ss;
 	const std::map<int, Signature *> & signatures = this->getSignatures();
@@ -155,15 +156,27 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		ss = this->_getSignature(i->first);
 		if(ss)
 		{
+			const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
+			if(words.size())
+			{
+				UDEBUG("node=%d, word references=%d", ss->id(), words.size());
+				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
+				{
+					_vwd->addWordRef(iter->first, i->first);
+				}
 				ss->setEnabled(true);
 			}
 		}
-
-	if(_vwd)
-	{
-		ULOGGER_DEBUG("Total word reference added = %d", _vwd->getTotalActiveReferences());
 	}
 
+	UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
+
+	if(_vwd->getUnusedWordsSize())
+	{
+		UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
+	}
+	ULOGGER_DEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
+
 	return success;
 }
 
@@ -857,6 +870,11 @@ void Memory::clear()
 		ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
 	}
 	_stMem.clear();
+	if(_signatures.size()!=0)
+	{
+		ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
+	}
+	_signatures.clear();
 
 	ULOGGER_DEBUG("");
 	// Wait until the db trash has finished cleaning the memory
@@ -2211,7 +2229,7 @@ void Memory::disableWordsRef(int signatureId)
 
 		count -= _vwd->getTotalActiveReferences();
 		ss->setEnabled(false);
-		ULOGGER_DEBUG("%d words total ref removed from signature %d...", count, ss->id());
+		ULOGGER_DEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
 	}
 }
 
@@ -2229,7 +2247,7 @@ void Memory::cleanUnusedWords()
 
 			for(unsigned int i=0; i<removedWords.size(); ++i)
 			{
-				if(_dbDriver && !removedWords[i]->isSaved())
+				if(_dbDriver)
 				{
 					_dbDriver->asyncSave(removedWords[i]);
 				}

corelib/src/VWDictionary.cpp

@@ -378,14 +378,26 @@ int VWDictionary::getNextId()
 }
 
 void VWDictionary::addWordRef(int wordId, int signatureId)
+{
+	if(signatureId > 0 && wordId > 0)
 	{
 		VisualWord * vw = 0;
 		vw = uValue(_visualWords, wordId, vw);
+		if(!vw)
+		{
+			vw = uValue(_unusedWords, wordId, vw);
+			_visualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
+			_unusedWords.erase(vw->id());
+		}
 		if(vw)
 		{
 			vw->addRef(signatureId);
 			_totalActiveReferences += 1;
-		_unusedWords.erase(vw->id());
+		}
+		else
+		{
+			UERROR("Not found word %d", wordId);
+		}
 	}
 }
 
@@ -812,10 +824,17 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
 void VWDictionary::addWord(VisualWord * vw)
 {
 	if(vw)
+	{
+		if(vw->getReferences().size())
 		{
 			_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
 			_totalActiveReferences += uSum(uValues(vw->getReferences()));
 		}
+		else
+		{
+			_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
+		}
+	}
 }
 
 // dist = (euclidean dist)^2, "k" nearest neighbors