0.20: added GlobalDescriptor table in database

corelib/include/rtabmap/core/DBDriver.h

@@ -100,7 +100,7 @@ public:
 
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
-	void addStatistics(const Statistics & statistics) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -238,7 +238,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const = 0;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -104,7 +104,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -160,6 +160,7 @@ private:
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
@@ -169,7 +170,8 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 			int nodeId,
 			const cv::Mat & ground,

corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/Memory.h

@@ -93,7 +93,7 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	int cleanup();
-	void saveStatistics(const Statistics & statistics);
+	void saveStatistics(const Statistics & statistics, bool saveWMState);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;

corelib/include/rtabmap/core/SensorData.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/EnvSensor.h>
 #include <rtabmap/core/Landmark.h>
+#include <rtabmap/core/GlobalDescriptor.h>
 
 namespace rtabmap
 {
@@ -249,6 +250,11 @@ public:
 	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void addGlobalDescriptor(const GlobalDescriptor & descriptor) {_globalDescriptors.push_back(descriptor);}
+	void setGlobalDescriptors(const std::vector<GlobalDescriptor> & descriptors) {_globalDescriptors = descriptors;}
+	void clearGlobalDescriptors() {_globalDescriptors.clear();}
+	const std::vector<GlobalDescriptor> & globalDescriptors() const {return _globalDescriptors;}
+
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
@@ -323,6 +329,9 @@ private:
 	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;
 
+	// global descriptors
+	std::vector<GlobalDescriptor> _globalDescriptors;
+
 	Transform groundTruth_;
 
 	Transform globalPose_;

corelib/src/DBDriver.cpp

@@ -1140,10 +1140,10 @@ void DBDriver::addInfoAfterRun(
 	}
 }
 
-void DBDriver::addStatistics(const Statistics & statistics) const
+void DBDriver::addStatistics(const Statistics & statistics, bool saveWmState) const
 {
 	_dbSafeAccessMutex.lock();
-	addStatisticsQuery(statistics);
+	addStatisticsQuery(statistics, saveWmState);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -3384,6 +3384,74 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
 		}
+
+		// load global descriptors
+		if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			std::stringstream query3;
+			query3 << "SELECT type, info, data "
+					 "FROM GlobalDescriptor "
+					 "WHERE node_id = ? ";
+
+			rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+			{
+				// bind id
+				rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				std::vector<GlobalDescriptor> globalDescriptors;
+
+				rc = sqlite3_step(ppStmt);
+				while(rc == SQLITE_ROW)
+				{
+					int index=0;
+					const void * data = 0;
+					int dataSize = 0;
+					int type = -1;
+					cv::Mat info;
+					cv::Mat dataMat;
+
+					type = sqlite3_column_int(ppStmt, index++);
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+
+					UASSERT(!dataMat.empty());
+					globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
+
+					rc = sqlite3_step(ppStmt);
+				}
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				if(!globalDescriptors.empty())
+				{
+					(*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
+					ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
+				}
+
+				//reset
+				rc = sqlite3_reset(ppStmt);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
+		}
+
 		if(ids.size() != loaded)
 		{
 			UERROR("Some signatures not found in database");
@@ -4235,6 +4303,27 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		UDEBUG("Time=%fs", timer.ticks());
 
+		if(uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			// Global descriptor table
+			std::string query = queryStepGlobalDescriptor();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+			{
+				for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
+				{
+					stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
+				}
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			UDEBUG("Time=%fs", timer.ticks());
+		}
+
 		if(uStrNumCmp(_version, "0.10.0") >= 0)
 		{
 			// Add SensorData
@@ -4519,7 +4608,7 @@ void DBDriverSqlite3::updateLaserScanQuery(
 	}
 }
 
-void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
+void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
 {
 	UDEBUG("Ref ID = %d", statistics.refImageId());
 	if(_ppDb)
@@ -4570,7 +4659,7 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				cv::Mat compressedWmState;
 				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
 				{
-					if(!statistics.wmState().empty())
+					if(saveWmState && !statistics.wmState().empty())
 					{
 						compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
 						rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
@@ -6364,6 +6453,61 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
+{
+	UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
+	return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
+}
+void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
+		int nodeId,
+		const GlobalDescriptor & descriptor) const
+{
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	//node_if
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//type
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//info
+	std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//data
+	std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);

corelib/src/Memory.cpp

@@ -2003,11 +2003,11 @@ int Memory::cleanup()
 	return signatureRemoved;
 }
 
-void Memory::saveStatistics(const Statistics & statistics)
+void Memory::saveStatistics(const Statistics & statistics, bool saveWmState)
 {
 	if(_dbDriver)
 	{
-		_dbDriver->addStatistics(statistics);
+		_dbDriver->addStatistics(statistics, saveWmState);
 	}
 }
 
@@ -5060,6 +5060,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());
+	s->sensorData().setGlobalDescriptors(data.globalDescriptors());
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

corelib/src/Rtabmap.cpp

@@ -3535,26 +3535,24 @@ bool Rtabmap::process(
 			UDEBUG("Computing RMSE...done!");
 		}
 
-		if(_saveWMState && _memory->isIncremental())
+		std::vector<int> ids;
+		ids.reserve(_memory->getWorkingMem().size() + _memory->getStMem().size());
+		for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
 		{
-			std::vector<int> ids = uKeys(_memory->getWorkingMem());
-			if(_memory->getStMem().size())
-			{
-				ids.resize(ids.size() + _memory->getStMem().size());
-				for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
-				{
-					ids.push_back(*iter);
-				}
-			}
-			statistics_.setWmState(ids);
+			ids.push_back(*iter);
 		}
-		UDEBUG("");
+		for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().lower_bound(0); iter!=_memory->getWorkingMem().end(); ++iter)
+		{
+			ids.push_back(iter->first);
+		}
+		statistics_.setWmState(ids);
+		UDEBUG("wmState=%d", (int)ids.size());
 	}
 
 	//Save statistics to database
 	if(_memory->isIncremental() || _memory->isLocalizationDataSaved())
 	{
-		_memory->saveStatistics(statistics_);
+		_memory->saveStatistics(statistics_, _saveWMState);
 	}
 
 	//Start trashing

corelib/src/resources/DatabaseSchema.sql.in

@@ -87,6 +87,14 @@ CREATE TABLE Feature (
 	FOREIGN KEY (node_id) REFERENCES Node(id)
 );
 
+CREATE TABLE GlobalDescriptor (
+	node_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,
+	info BLOB,
+	data BLOB NOT NULL,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
 --
 
 CREATE TABLE Info (
@@ -162,6 +170,7 @@ END;
 -- *******************************************************************
 CREATE UNIQUE INDEX IDX_Node_id on Node (id);
 CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
 CREATE INDEX IDX_Link_from_id on Link (from_id);
 CREATE UNIQUE INDEX IDX_node_label on Node (label);
 CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);