Database update (version 0.8.5): added Node.stamp and Node.label fields

Added labeling mechanism of nodes (by default the first node of a map is tagged "map#")

corelib/src/CameraThread.cpp

@@ -125,11 +125,11 @@ void CameraThread::mainLoop()
 	{
 		if(_cameraRGBD)
 		{
-			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
+			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq, UTimer::now()));
 		}
 		else
 		{
-			this->post(new CameraEvent(rgb, ++_seq));
+			this->post(new CameraEvent(rgb, ++_seq, UTimer::now()));
 		}
 	}
 	else if(!this->isKilled())

corelib/src/DBDriver.cpp

@@ -483,6 +483,41 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
+{
+	if(!label.empty())
+	{
+		int idFound = 0;
+		// look in the trash
+		_trashesMutex.lock();
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			if(sIter->second->getLabel().compare(label) == 0)
+			{
+				idFound = sIter->first;
+				break;
+			}
+		}
+		_trashesMutex.unlock();
+
+		// then look in the database
+		if(idFound == 0)
+		{
+			_dbSafeAccessMutex.lock();
+			this->getNodeIdByLabelQuery(label, id);
+			_dbSafeAccessMutex.unlock();
+		}
+		else
+		{
+			id = idFound;
+		}
+	}
+	else
+	{
+		UWARN("Can't search with an empty label!");
+	}
+}
+
 void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
 {
 	ULOGGER_DEBUG("");

corelib/src/DBDriverSqlite3.cpp

@@ -927,6 +927,36 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
 	}
 }
 
+void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
+{
+	if(_ppDb && !label.empty())
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+		query << "SELECT id FROM Node WHERE label='" << label <<"'";
+
+		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);
+		if(rc == SQLITE_ROW)
+		{
+			id = sqlite3_column_int(ppStmt, 0);
+			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;
@@ -976,9 +1006,18 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		unsigned int loaded = 0;
 
 		// Load nodes information
-		query << "SELECT id, map_id, weight, pose "
-			  << "FROM Node "
-			  << "WHERE id=?;";
+		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			query << "SELECT id, map_id, weight, pose, stamp, label "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else
+		{
+			query << "SELECT id, map_id, weight, pose "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
 
 		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());
@@ -992,10 +1031,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			int id = 0;
 			int mapId = 0;
+			double stamp = 0.0;
 			int weight = 0;
 			Transform pose;
 			const void * data = 0;
 			int dataSize = 0;
+			std::string label;
 
 			// Process the result if one
 			rc = sqlite3_step(ppStmt);
@@ -1012,6 +1053,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				{
 					memcpy(pose.data(), data, dataSize);
 				}
+
+				if(uStrNumCmp(_version, "0.8.5") >= 0)
+				{
+					stamp = sqlite3_column_double(ppStmt, index++);
+					const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+					if(p)
+					{
+						label = reinterpret_cast<const char*>(p);
+					}
+				}
+
 				rc = sqlite3_step(ppStmt);
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1023,10 +1075,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				Signature * s = new Signature(
 						id,
 						mapId,
+						weight,
+						stamp,
+						label,
 						std::multimap<int, cv::KeyPoint>(),
 						std::multimap<int, pcl::PointXYZ>(),
 						pose);
-				s->setWeight(weight);
 				s->setSaved(true);
 				nodes.push_back(s);
 				++loaded;
@@ -1583,13 +1637,27 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		Signature * s = 0;
 
 		std::string query;
-		if(updateTimestamp)
+		if(uStrNumCmp(_version, "0.8.5") >= 0)
 		{
-			query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
+			}
 		}
 		else
 		{
-			query = "UPDATE Node SET weight=? WHERE id=?;";
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=? 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());
@@ -1603,6 +1671,20 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+				if(uStrNumCmp(_version, "0.8.5") >= 0)
+				{
+					if(s->getLabel().empty())
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+					else
+					{
+						rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+				}
+
 				rc = sqlite3_bind_int(ppStmt, index++, s->id());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -1900,6 +1982,10 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 
 std::string DBDriverSqlite3::queryStepNode() const
 {
+	if(uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
+	}
 	return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
 }
 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
@@ -1921,6 +2007,23 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 	rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		if(s->getLabel().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
+			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());

corelib/src/DBDriverSqlite3.h

@@ -86,6 +86,7 @@ private:
 	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;
 
 private:
 	std::string queryStepNode() const;

corelib/src/DBReader.cpp

@@ -233,7 +233,8 @@ SensorData DBReader::getNextData()
 					pose,
 					rotVariance,
 					transVariance,
-					seq);
+					seq,
+					UTimer::now());
 			UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
 					data.laserScan().empty()?0:1,
 					data.image().empty()?0:1,

corelib/src/Memory.cpp

@@ -689,6 +689,24 @@ void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, flo
 				UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
 						*_stMem.rbegin(), signature->id(),
 						_signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
+
+				//Tag the first node of the map
+				std::string tag = uFormat("map%d", signature->mapId());
+				if(getSignatureIdByLabel(tag, false) == 0)
+				{
+					UINFO("Tagging node %d with label \"%s\"", tag.c_str());
+					signature->setLabel(tag);
+				}
+			}
+		}
+		else
+		{
+			//Tag the first node of the map
+			std::string tag = uFormat("map%d", signature->mapId());
+			if(getSignatureIdByLabel(tag, false) == 0)
+			{
+				UINFO("Tagging node %d with label \"%s\"", tag.c_str());
+				signature->setLabel(tag);
 			}
 		}
 
@@ -1580,6 +1598,64 @@ const Signature * Memory::getLastWorkingSignature() const
 	return _lastSignature;
 }
 
+int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
+{
+	int id = 0;
+	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		if(iter->second->getLabel().compare(label) == 0)
+		{
+			id = iter->second->id();
+			break;
+		}
+	}
+	if(id == 0 && _dbDriver && lookInDatabase)
+	{
+		_dbDriver->getNodeIdByLabel(label, id);
+	}
+	return id;
+}
+
+bool Memory::labelSignature(int id, const std::string & label)
+{
+	if(!label.empty())
+	{
+		// verify that this label is not used
+		int idFound=getSignatureIdByLabel(label);
+		if(idFound == 0 || idFound == id)
+		{
+			Signature * s  = this->_getSignature(id);
+			if(s)
+			{
+				s->setLabel(label);
+				return true;
+			}
+			else if(_dbDriver)
+			{
+				std::list<int> ids;
+				ids.push_back(id);
+				std::list<Signature *> signatures;
+				_dbDriver->loadSignatures(ids,signatures);
+				if(signatures.size())
+				{
+					signatures.front()->setLabel(label);
+					_dbDriver->asyncSave(signatures.front()); // move it again to trash
+					return true;
+				}
+			}
+			else
+			{
+				UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
+			}
+		}
+		else if(idFound)
+		{
+			UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+		}
+	}
+	return false;
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -3509,6 +3585,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 
 		s = new Signature(id,
 			_idMapCount,
+			0,
+			data.stamp(),
+			"",
 			words,
 			words3D,
 			data.pose(),
@@ -3525,6 +3604,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	{
 		s = new Signature(id,
 			_idMapCount,
+			0,
+			data.stamp(),
+			"",
 			words,
 			words3D,
 			data.pose(),

corelib/src/Rtabmap.cpp

@@ -599,6 +599,26 @@ int Rtabmap::triggerNewMap()
 	return mapId;
 }
 
+bool Rtabmap::labelLocation(int id, const std::string & label)
+{
+	if(!label.empty() && _memory)
+	{
+		if(id > 0)
+		{
+			return _memory->labelSignature(id, label);
+		}
+		else if(_memory->getLastWorkingSignature())
+		{
+			return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
+		}
+		else
+		{
+			UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
+		}
+	}
+	return false;
+}
+
 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
 {
 	if(_memory)

corelib/src/SensorData.cpp

@@ -38,6 +38,7 @@ namespace rtabmap
 SensorData::SensorData() :
 	_image(cv::Mat()),
 	_id(0),
+	_stamp(0.0),
 	_fx(0.0f),
 	_fyOrBaseline(0.0f),
 	_cx(0.0f),
@@ -49,9 +50,11 @@ SensorData::SensorData() :
 }
 
 SensorData::SensorData(const cv::Mat & image,
-	  int id) :
+	  int id,
+	  double stamp) :
 	_image(image),
 	_id(id),
+	_stamp(stamp),
 	_fx(0.0f),
 	_fyOrBaseline(0.0f),
 	_cx(0.0f),
@@ -75,9 +78,11 @@ SensorData::SensorData(const cv::Mat & image,
 		  const Transform & pose,
 		  float poseRotVariance,
 		  float poseTransVariance,
-		  int id) :
+		  int id,
+		  double stamp) :
 	_image(image),
 	_id(id),
+	_stamp(stamp),
 	_depthOrRightImage(depthOrRightImage),
 	_fx(fx),
 	_fyOrBaseline(fyOrBaseline),
@@ -109,9 +114,11 @@ SensorData::SensorData(const cv::Mat & laserScan,
 		  const Transform & pose,
 		  float poseRotVariance,
 		  float poseTransVariance,
-		  int id) :
+		  int id,
+		  double stamp) :
 	_image(image),
 	_id(id),
+	_stamp(stamp),
 	_depthOrRightImage(depthOrRightImage),
 	_laserScan(laserScan),
 	_fx(fx),

corelib/src/Signature.cpp

@@ -39,6 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
+	_stamp(0.0),
 	_weight(-1),
 	_saved(false),
 	_modified(true),
@@ -54,6 +55,9 @@ Signature::Signature() :
 Signature::Signature(
 		int id,
 		int mapId,
+		int weight,
+		double stamp,
+		const std::string & label,
 		const std::multimap<int, cv::KeyPoint> & words,
 		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
@@ -67,7 +71,9 @@ Signature::Signature(
 		const Transform & localTransform) :
 	_id(id),
 	_mapId(mapId),
-	_weight(0),
+	_stamp(stamp),
+	_weight(weight),
+	_label(label),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
@@ -258,7 +264,8 @@ SensorData Signature::toSensorData()
 			_pose,
 			rotVariance,
 			transVariance,
-			_id);
+			_id,
+			_stamp);
 }
 
 void Signature::uncompressData()

corelib/src/resources/DatabaseSchema.sql.in

@@ -17,7 +17,9 @@ CREATE TABLE Node (
 	id INTEGER NOT NULL,
 	map_id INTEGER NOT NULL,
 	weight INTEGER,
+	stamp FLOAT,
 	pose BLOB,
+	label TEXT,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
@@ -122,6 +124,7 @@ END;
 -- *******************************************************************
 CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
 CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
 
 -- *******************************************************************
 -- VERSION