Added Memory::getAllLabels(), added labels to statistics and added DBDriver::getNodeInfo()

corelib/src/DBDriver.cpp

@@ -474,7 +474,7 @@ void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
 	}
 }
 
-void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
+bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const
 {
 	bool found = false;
 	// look in the trash
@@ -483,6 +483,9 @@ void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
 	{
 		pose = _trashSignatures.at(signatureId)->getPose();
 		mapId = _trashSignatures.at(signatureId)->mapId();
+		weight = _trashSignatures.at(signatureId)->getWeight();
+		label = _trashSignatures.at(signatureId)->getLabel();
+		stamp = _trashSignatures.at(signatureId)->getStamp();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -490,9 +493,10 @@ void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		this->getPoseQuery(signatureId, pose, mapId);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
 		_dbSafeAccessMutex.unlock();
 	}
+	return found;
 }
 
 void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
@@ -668,6 +672,25 @@ void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
 	}
 }
 
+void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+	{
+		if(!sIter->second->getLabel().empty())
+		{
+			labels.insert(std::make_pair(sIter->first, sIter->second->getLabel()));
+		}
+	}
+	_trashesMutex.unlock();
+
+	// then look in the database
+	_dbSafeAccessMutex.lock();
+	this->getAllLabelsQuery(labels);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
 {
 	ULOGGER_DEBUG("");

corelib/src/DBDriverSqlite3.cpp

@@ -751,8 +751,14 @@ void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompresse
 	}
 }
 
-void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapId) const
+bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
+		Transform & pose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp) const
 {
+	bool found = false;
 	if(_ppDb && signatureId)
 	{
 		int rc = SQLITE_OK;
@@ -760,10 +766,20 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		std::stringstream query;
 
 		// Prepare the query... Get the map from signature and visual words
-		query << "SELECT pose, map_id "
-				 "FROM Node "
-				 "WHERE id = " << signatureId <<
-				 ";";
+		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			query << "SELECT pose, map_id, weight, label "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else
+		{
+			query << "SELECT pose, map_id, weight "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -775,6 +791,7 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		rc = sqlite3_step(ppStmt);
 		if(rc == SQLITE_ROW)
 		{
+			found = true;
 			int index = 0;
 			data = sqlite3_column_blob(ppStmt, index); // pose
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -784,7 +801,16 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 			}
 
 			mapId = sqlite3_column_int(ppStmt, index++); // map id
+			weight = sqlite3_column_int(ppStmt, index++); // weight
 
+			if(uStrNumCmp(_version, "0.8.5") >= 0)
+			{
+				const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+				if(p)
+				{
+					label = reinterpret_cast<const char*>(p); // label
+				}
+			}
 
 			rc = sqlite3_step(ppStmt); // next result...
 		}
@@ -794,6 +820,7 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
+	return found;
 }
 
 
@@ -929,7 +956,7 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
 
 void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
 {
-	if(_ppDb && !label.empty())
+	if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
 	{
 		UTimer timer;
 		timer.start();
@@ -957,6 +984,46 @@ void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id)
 	}
 }
 
+void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
+{
+	if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+		query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int index = 0;
+			int id = sqlite3_column_int(ppStmt, index++);
+			const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+			if(p)
+			{
+				std::string label = reinterpret_cast<const char*>(p);
+				if(!label.empty())
+				{
+					labels.insert(std::make_pair(id, label));
+				}
+			}
+			rc = sqlite3_step(ppStmt);
+		}
+		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());
+		ULOGGER_DEBUG("Time=%f", timer.ticks());
+	}
+}
+
 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
 {
 	weight = 0;

corelib/src/DBDriverSqlite3.h

@@ -82,11 +82,12 @@ private:
 			float & cy,
 			Transform & localTransform) const;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
-	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
 private:
 	std::string queryStepNode() const;

corelib/src/DBReader.cpp

@@ -198,7 +198,10 @@ SensorData DBReader::getNextData()
 			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
 			if(!_odometryIgnored)
 			{
-				_dbDriver->getPose(*_currentId, pose, mapId);
+				int weight;
+				std::string label;
+				double stamp;
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
 				std::map<int, Link> links;
 				_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
 				if(links.size())

corelib/src/Memory.cpp

@@ -754,21 +754,6 @@ const VWDictionary * Memory::getVWDictionary() const
 	return _vwd;
 }
 
-void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) const
-{
-	const Signature * s = getSignature(locationId);
-	int mapId = -1;
-	if(s)
-	{
-		pose = s->getPose();
-		mapId = s->mapId();
-	}
-	else if(lookInDatabase && _dbDriver)
-	{
-		_dbDriver->getPose(locationId, pose, mapId);
-	}
-}
-
 std::map<int, Link> Memory::getNeighborLinks(
 		int signatureId,
 		bool lookInDatabase) const
@@ -1690,6 +1675,23 @@ bool Memory::labelSignature(int id, const std::string & label)
 	return false;
 }
 
+std::map<int, std::string> Memory::getAllLabels() const
+{
+	std::map<int, std::string> labels;
+	for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		if(!iter->second->getLabel().empty())
+		{
+			labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
+		}
+	}
+	if(_dbDriver)
+	{
+		_dbDriver->getAllLabels(labels);
+	}
+	return labels;
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -2692,20 +2694,39 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 	return false;
 }
 
-int Memory::getMapId(int signatureId) const
+Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
+{
+	Transform pose;
+	int mapId, weight;
+	std::string label;
+	double stamp;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase);
+	return pose;
+}
+
+bool Memory::getNodeInfo(int signatureId,
+		Transform & odomPose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp,
+		bool lookInDatabase) const
 {
-	int mapId = 0;
 	const Signature * s = this->getSignature(signatureId);
 	if(s)
 	{
+		odomPose = s->getPose();
 		mapId = s->mapId();
+		weight = s->getWeight();
+		label = s->getLabel();
+		stamp = s->getStamp();
+		return true;
 	}
-	else if(_dbDriver)
+	else if(lookInDatabase && _dbDriver)
 	{
-		Transform pose;
-		_dbDriver->getPose(signatureId, pose, mapId);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp);
 	}
-	return mapId;
+	return false;
 }
 
 cv::Mat Memory::getImageCompressed(int signatureId) const
@@ -3868,8 +3889,7 @@ void Memory::getMetricConstraints(
 	UDEBUG("");
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
-		Transform pose;
-		this->getPose(ids[i], pose, lookInDatabase);
+		Transform pose = getOdomPose(ids[i], lookInDatabase);
 		if(!pose.isNull())
 		{
 			poses.insert(std::make_pair(ids[i], pose));

corelib/src/Rtabmap.cpp

@@ -1586,6 +1586,7 @@ bool Rtabmap::process(const SensorData & data)
 			}
 		}
 	}
+
 	//============================================================
 	// Prepare statistics
 	//============================================================
@@ -1654,15 +1655,24 @@ bool Rtabmap::process(const SensorData & data)
 				std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
 				std::map<int, Transform> poses;
 				std::map<int, int> mapIds;
+				std::map<int, std::string> labels;
 				std::multimap<int, Link> constraints;
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
 				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
-					mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+					Transform odomPose;
+					int weight = -1;
+					int mapId = -1;
+					std::string label;
+					double stamp = 0;
+					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false);
+					mapIds.insert(std::make_pair(iter->first, mapId));
+					labels.insert(std::make_pair(iter->first, label));
 				}
 				statistics_.setPoses(poses);
 				statistics_.setConstraints(constraints);
 				statistics_.setMapIds(mapIds);
+				statistics_.setLabels(labels);
 			}
 			else // RGBD-SLAM mode
 			{
@@ -1805,9 +1815,6 @@ bool Rtabmap::process(const SensorData & data)
 	timeRealTimeLimitReachedProcess = timer.ticks();
 	ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
 
-	//Start trashing
-	_memory->emptyTrash();
-
 	//==============================================================
 	// Finalize statistics and log files
 	//==============================================================
@@ -1828,17 +1835,29 @@ bool Rtabmap::process(const SensorData & data)
 		if(_rgbdSlamMode)
 		{
 			std::map<int, int> mapIds;
+			std::map<int, std::string> labels;
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
-				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+				Transform odomPose;
+				int weight = -1;
+				int mapId = -1;
+				std::string label;
+				double stamp = 0;
+				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+				mapIds.insert(std::make_pair(iter->first, mapId));
+				labels.insert(std::make_pair(iter->first, label));
 			}
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapIds(mapIds);
+			statistics_.setLabels(labels);
 		}
 
 	}
 
+	//Start trashing
+	_memory->emptyTrash();
+
 	// Log info...
 	// TODO : use a specific class which will handle the RtabmapEvent
 	if(_foutFloat && _foutInt)
@@ -2271,6 +2290,7 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		std::map<int, int> & mapIds,
+		std::map<int, std::string> & labels,
 		bool optimized,
 		bool global) const
 {
@@ -2298,7 +2318,14 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			Transform odomPose;
+			int weight = -1;
+			int mapId = -1;
+			std::string label;
+			double stamp = 0;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+			mapIds.insert(std::make_pair(iter->first, mapId));
+			labels.insert(std::make_pair(iter->first, label));
 		}
 
 
@@ -2337,6 +2364,7 @@ void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		std::map<int, int> & mapIds,
+		std::map<int, std::string> & labels,
 		bool optimized,
 		bool global)
 {
@@ -2363,7 +2391,14 @@ void Rtabmap::getGraph(
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			Transform odomPose;
+			int weight = -1;
+			int mapId = -1;
+			std::string label;
+			double stamp = 0;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+			mapIds.insert(std::make_pair(iter->first, mapId));
+			labels.insert(std::make_pair(iter->first, label));
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -2471,7 +2506,8 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	std::map<int, Transform> nodes;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
-	this->getGraph(nodes, constraints, mapIds, true, global);
+	std::map<int, std::string> labels;
+	this->getGraph(nodes, constraints, mapIds, labels, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	if(computePath(targetNode, nodes, constraints))
@@ -2499,7 +2535,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	std::map<int, Transform> nodes;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
-	this->getGraph(nodes, constraints, mapIds, true, global);
+	std::map<int, std::string> labels;
+	this->getGraph(nodes, constraints, mapIds, labels, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

corelib/src/RtabmapThread.cpp

@@ -110,18 +110,21 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 
 	_rtabmap->get3DMap(signatures,
 			poses,
 			constraints,
 			mapIds,
+			labels,
 			optimized,
 			full);
 
 	this->post(new RtabmapEvent3DMap(signatures,
 			poses,
 			constraints,
-			mapIds));
+			mapIds,
+			labels));
 }
 
 void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
@@ -130,17 +133,20 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, std::string> labels;
 
 	_rtabmap->getGraph(poses,
 			constraints,
 			mapIds,
+			labels,
 			optimized,
 			full);
 
 	this->post(new RtabmapEvent3DMap(signatures,
 			poses,
 			constraints,
-			mapIds));
+			mapIds,
+			labels));
 }