Added Mem/LocalizationDataSaved parameter

corelib/include/rtabmap/core/DBDriver.h

@@ -173,6 +173,7 @@ public:
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
+	void getLastMapId(int & mapId) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
 	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
@@ -274,7 +275,7 @@ protected:
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
 	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -143,7 +143,7 @@ protected:
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
 	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;

corelib/include/rtabmap/core/Memory.h

@@ -209,6 +209,7 @@ public:
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
+	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
 	const Signature * getSignature(int id) const;
 	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
@@ -291,6 +292,7 @@ private:
 	bool _saveIntermediateNodeData;
 	std::string _rgbCompressionFormat;
 	bool _incrementalMemory;
+	bool _localizationDataSaved;
 	bool _reduceGraph;
 	int _maxStMemSize;
 	float _recentWmRatio;

corelib/include/rtabmap/core/Parameters.h

@@ -207,6 +207,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
+    RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
     RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");

corelib/src/DBDriver.cpp

@@ -935,6 +935,21 @@ void DBDriver::getLastNodeId(int & id) const
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getLastMapId(int & mapId) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		mapId = _trashSignatures.rbegin()->second->mapId();
+	}
+	_trashesMutex.unlock();
+
+	_dbSafeAccessMutex.lock();
+	this->getLastIdQuery("Node", mapId, "map_id");
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::getLastWordId(int & id) const
 {
 	// look in the trash

corelib/src/DBDriverSqlite3.cpp

@@ -2555,18 +2555,18 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 	}
 }
 
-void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id) const
+void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName) const
 {
 	if(_ppDb)
 	{
-		UDEBUG("get last id from table \"%s\"", tableName.c_str());
+		UDEBUG("get last %s from table \"%s\"", fieldName.c_str(), tableName.c_str());
 		UTimer timer;
 		timer.start();
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT max(id) "
+		query << "SELECT max(" << fieldName << ") "
 			  << "FROM " << tableName
 			  << ";";
 

corelib/src/Memory.cpp

@@ -80,6 +80,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
+	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
 	_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -345,7 +346,9 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 
 		// Last id
 		_dbDriver->getLastNodeId(_idCount);
-		_idMapCount = _lastSignature?_lastSignature->mapId()+1:kIdStart;
+		_idMapCount = -1;
+		_dbDriver->getLastMapId(_idMapCount);
+		++_idMapCount;
 
 		// Now load the dictionary if we have a connection
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
@@ -571,6 +574,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers);
 	Parameters::parse(params, Parameters::kMarkerVarianceLinear(), _markerLinVariance);
 	Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
+	Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -864,7 +868,7 @@ bool Memory::update(
 	}
 	if(stats) stats->setReducedIds(reducedIds);
 
-	if(!_memoryChanged && _incrementalMemory)
+	if(!_memoryChanged && (_incrementalMemory || _localizationDataSaved))
 	{
 		_memoryChanged = true;
 	}
@@ -2407,7 +2411,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
 			_dbDriver &&
 			s->id()>0 &&
-			(_incrementalMemory || s->isSaved()))
+			(_incrementalMemory || s->isSaved() || _localizationDataSaved))
 		{
 			if(keepLinkedToGraph)
 			{

corelib/src/Rtabmap.cpp

@@ -3541,7 +3541,7 @@ bool Rtabmap::process(
 	}
 
 	//Save statistics to database
-	if(_memory->isIncremental())
+	if(_memory->isIncremental() || _memory->isLocalizationDataSaved())
 	{
 		_memory->saveStatistics(statistics_);
 	}