-Updated File menu with new actions "New database...", "Open database", "Close database" and "Edit database". Each new session will be automatically saved when closing the database. The "play" action cannot be activated until a new database is created or a previous session database is opened.

- Memory usage reviewed for the GUI: we don't uncompress data for downloaded nodes from "Download all clouds" action. - Odometry: added optional argument "initialPose" to reset() method git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1930 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-02 01:20:25 +08:00 · 2014-10-28 01:20:57 +00:00
parent 457c068e0f
commit b56e03b463
30 changed files with 632 additions and 424 deletions
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -91,7 +91,7 @@ public:
 	// Load objects
 	void load(VWDictionary * dictionary) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);

 	// Specific queries...
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -69,7 +69,7 @@ public:
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
-			bool postInitEvents = true);
+			bool postInitClosingEvents = false);
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId();
@@ -215,6 +215,7 @@ private:
 	int _lastGlobalLoopClosureChildId;
 	bool _memoryChanged; // False by default, become true when Memory::update() is called.
 	int _signaturesAdded;
+	bool _postInitClosingEvents;

 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -57,7 +57,7 @@ class RTABMAP_EXP Odometry
 public:
 	virtual ~Odometry() {}
 	Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
-	virtual void reset();
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 	bool isLargeEnoughTransform(const Transform & transform);

@@ -104,7 +104,7 @@ public:
 	OdometryBOW(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
 	virtual ~OdometryBOW();

-	virtual void reset();
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	const std::multimap<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
 	const Memory * getMemory() const {return _memory;}

@@ -125,7 +125,7 @@ public:
 	OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
 	virtual ~OdometryOpticalFlow();

-	virtual void reset();
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 	const cv::Mat & getLastFrame() const {return lastFrame_;}
 	const std::vector<cv::Point2f> & getLastCorners() const {return lastCorners_;}
@@ -168,7 +168,7 @@ public:
 			float maxFitness = 0.01f,
 			bool pointToPlane = true,
 			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
-	void reset();
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 private:
 	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -139,7 +139,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              int, 1, 	 "Data buffer size (0 min inf).");
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
-	RTABMAP_PARAM_STR(Rtabmap, DatabasePath,     Parameters::getDefaultDatabasePath(), "Database path.");
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,                 unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
@@ -354,7 +353,6 @@ public:
 private:
 	Parameters();
 	static std::string getDefaultWorkingDirectory();
-	static std::string getDefaultDatabasePath();

 private:
 	static ParametersMap parameters_;
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -65,13 +65,12 @@ public:
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
 	bool process(const SensorData & data); // for convenience

-	void init(const ParametersMap & param, bool deleteMemory = true);
-	void init(const std::string & configFile = "", bool deleteMemory = true);
+	void init(const ParametersMap & parameters, const std::string & databasePath = "");
+	void init(const std::string & configFile = "", const std::string & databasePath = "");

 	void close();

 	const std::string & getWorkingDir() const {return _wDir;}
-	std::string getDatabasePath() const;
 	int getLoopClosureId() const;
 	int getRetrievedId() const;
 	int getLastLocationId() const;
@@ -98,12 +97,11 @@ public:
 	void triggerNewMap();
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
 	void generateTOROGraph(const std::string & path, bool optimized, bool global);
-	void resetMemory(bool dbOverwritten = false);
+	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
-	void setDatabasePath(const std::string & path);
 	void deleteLocation(int locationId); // Only nodes in STM can be deleted
 	void rejectLoopClosure(int oldId, int newId);
 	void get3DMap(std::map<int, Signature> & signatures,
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/corelib/include/rtabmap/core/RtabmapEvent.h
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/utilite/UEvent.h>
 #include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Parameters.h"

 namespace rtabmap
 {
@@ -57,14 +58,15 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
+			kCmdInit,
 			kCmdResetMemory,
+			kCmdClose,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: path
 			kCmdGenerateDOTLocalGraph, // params: path, id, margin
 			kCmdGenerateTOROGraphLocal, // params: path, optimized
 			kCmdGenerateTOROGraphGlobal, // params: path, optimized
-			kCmdDeleteMemory, // params: path [optional]
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMapLocal, // params: optimized
 			kCmdPublish3DMapGlobal, // params: optimized
@@ -73,26 +75,12 @@ public:
 			kCmdTriggerNewMap,
 			kCmdPause};
 public:
-	RtabmapEventCmd(Cmd cmd) :
-		UEvent(0),
-		_cmd(cmd),
-		_strValue(""),
-		_intValue(0){}
-	RtabmapEventCmd(Cmd cmd, int value) :
-			UEvent(0),
-			_cmd(cmd),
-			_strValue(""),
-			_intValue(value){}
-	RtabmapEventCmd(Cmd cmd, const std::string & value) :
-			UEvent(0),
-			_cmd(cmd),
-			_strValue(value),
-			_intValue(0){}
-	RtabmapEventCmd(Cmd cmd, const std::string & strValue, int intValue) :
+	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
 			_cmd(cmd),
 			_strValue(strValue),
-			_intValue(intValue){}
+			_intValue(intValue),
+			_parameters(parameters){}

 	virtual ~RtabmapEventCmd() {}
 	Cmd getCmd() const {return _cmd;}
@@ -103,12 +91,15 @@ public:
 	void setInt(int v) {_intValue = v;}
 	int getInt() const {return _intValue;}

+	const ParametersMap & getParameters() const {return _parameters;}
+
 	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}

 private:
 	Cmd _cmd;
 	std::string _strValue;
 	int _intValue;
+	ParametersMap _parameters;
 };

 class RtabmapEventInit : public UEvent
@@ -118,6 +109,8 @@ public:
 	enum Status {
 		kInitializing,
 		kInitialized,
+		kClosing,
+		kClosed,
 		kInfo,
 		kError
 	};
--- a/corelib/include/rtabmap/core/RtabmapThread.h
+++ b/corelib/include/rtabmap/core/RtabmapThread.h
@@ -53,9 +53,10 @@ class RTABMAP_EXP RtabmapThread :
 {
 public:
 	enum State {
-		kStateIdle,
+		kStateInit,
 		kStateDetecting,
 		kStateReseting,
+		kStateClose,
 		kStateChangingParameters,
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
@@ -63,13 +64,11 @@ public:
 		kStateGeneratingDOTLocalGraph,
 		kStateGeneratingTOROGraphLocal,
 		kStateGeneratingTOROGraphGlobal,
-		kStateDeletingMemory,
 		kStateCleanDataBuffer,
 		kStatePublishingMapLocal,
 		kStatePublishingMapGlobal,
 		kStatePublishingTOROGraphLocal,
 		kStatePublishingTOROGraphGlobal,
-		Graph,
 		kStateTriggeringMap
 	};

--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -140,6 +140,7 @@ public:

 	SensorData toSensorData();
 	void uncompressData();
+	void uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const;

 private:
 	int _id;
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -71,12 +71,13 @@ class RTABMAP_EXP CompressionThread : public UThread
 public:
 	// format : ".png" ".jpg" "" (empty is general)
 	CompressionThread(const cv::Mat & mat, const std::string & format = "");
-	CompressionThread(const std::vector<unsigned char> & bytes, bool isImage);
+	CompressionThread(const std::vector<unsigned char> * bytes, bool isImage);
 	const std::vector<unsigned char> & getCompressedData() const {return compressedData_;}
 	cv::Mat & getUncompressedData() {return uncompressedData_;}
 protected:
 	virtual void mainLoop();
 private:
+	const std::vector<unsigned char> * constCompressedData_;
 	std::vector<unsigned char> compressedData_;
 	cv::Mat uncompressedData_;
 	std::string format_;
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -295,7 +295,9 @@ void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
 	_dbSafeAccessMutex.unlock();
 }

-void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
+void DBDriver::loadSignatures(const std::list<int> & signIds,
+		std::list<Signature *> & signatures,
+		std::set<int> * loadedFromTrash)
 {
 	UDEBUG("");
 	// look up in the trash before the database
@@ -326,6 +328,10 @@ void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signatur
 			}
 			if(valueFound)
 			{
+				if(loadedFromTrash)
+				{
+					loadedFromTrash->insert(*iter);
+				}
 				iter = ids.erase(iter);
 			}
 			else
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -220,9 +220,9 @@ void DBReader::getNextImage(
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}

-			util3d::CompressionThread ctImage(imageBytes, true);
-			util3d::CompressionThread ctDepth(depthBytes, true);
-			util3d::CompressionThread ctDepth2D(depth2dBytes, false);
+			util3d::CompressionThread ctImage(&imageBytes, true);
+			util3d::CompressionThread ctDepth(&depthBytes, true);
+			util3d::CompressionThread ctDepth2D(&depth2dBytes, false);
 			ctImage.start();
 			ctDepth.start();
 			ctDepth2D.start();
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -71,6 +71,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_lastGlobalLoopClosureChildId(0),
 	_memoryChanged(false),
 	_signaturesAdded(0),
+	_postInitClosingEvents(false),

 	_feature2D(new SURF(parameters)),
 	_featureType(Feature2D::kFeatureSurf),
@@ -114,16 +115,17 @@ Memory::Memory(const ParametersMap & parameters) :
 	this->parseParameters(parameters);
 }

-bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitEvents)
+bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
 {
-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
+	_postInitClosingEvents = postInitClosingEvents;
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));

 	UDEBUG("");
 	this->parseParameters(parameters);
 	bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
 	Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);

-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 	DBDriver * tmpDriver = 0;
 	if(!_memoryChanged)
 	{
@@ -134,7 +136,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		}
 	}
 	this->clear();
-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));

 	if(tmpDriver)
 	{
@@ -143,9 +145,9 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter

 	if(_dbDriver)
 	{
-		if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
 		_dbDriver->closeConnection();
-		if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
 	}

 	if(_dbDriver == 0 && !dbUrl.empty())
@@ -157,18 +159,18 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	if(_dbDriver)
 	{
 		success = false;
-		if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
 		if(_dbDriver->openConnection(dbUrl, dbOverwritten))
 		{
 			success = true;
-			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));

 			// Load the last working memory...
 			std::list<Signature*> dbSignatures;

 			if(loadAllNodesInWM)
 			{
-				if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
+				if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
 				std::set<int> ids;
 				_dbDriver->getAllNodeIds(ids, true);
 				_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
@@ -176,7 +178,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			else
 			{
 				// load previous session working memory
-				if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
+				if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
 				_dbDriver->loadLastNodes(dbSignatures);
 			}
 			for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
@@ -199,7 +201,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 					delete *iter;
 				}
 			}
-			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));

 			// Assign the last signature
 			if(_stMem.size()>0)
@@ -217,7 +219,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		}
 		else
 		{
-			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
 		}
 	}
 	else
@@ -235,7 +237,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	// Now load the dictionary if we have a connection
 	if(_dbDriver && _dbDriver->isConnected())
 	{
-		if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
 		if(loadAllNodesInWM)
 		{
 			// load all referenced words in working memory
@@ -268,10 +270,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
 		_vwd->update();
-		if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
 	}

-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
 	// Enable loaded signatures
 	const std::map<int, Signature *> & signatures = this->getSignatures();
 	for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -290,7 +292,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			s->setEnabled(true);
 		}
 	}
-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));

 	if(_vwd->getUnusedWordsSize())
 	{
@@ -298,34 +300,47 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	}
 	UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());

-	if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 	return success;
 }

 Memory::~Memory()
 {
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
 	UDEBUG("");
 	if(!_memoryChanged)
 	{
 		UDEBUG("");
 		if(_dbDriver)
 		{
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
 			_dbDriver->closeConnection();
 			delete _dbDriver;
 			_dbDriver = 0;
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
 		}
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 		this->clear();
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
 	}
 	else
 	{
 		UDEBUG("");
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
 		this->clear();
 		if(_dbDriver)
 		{
 			_dbDriver->emptyTrashes();
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
 			_dbDriver->closeConnection();
 			delete _dbDriver;
 			_dbDriver = 0;
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
+		}
+		else
+		{
+			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
 		}
 	}

@@ -337,6 +352,7 @@ Memory::~Memory()
 	{
 		delete _vwd;
 	}
+	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
 }

 void Memory::parseParameters(const ParametersMap & parameters)
@@ -2557,6 +2573,7 @@ std::vector<unsigned char> Memory::getImage(int signatureId) const

 Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 {
+	UDEBUG("");
 	Signature r;
 	Signature * s = this->_getSignature(locationId);
 	if(s && s->getImage().size())
@@ -2578,7 +2595,8 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 			std::list<int> ids;
 			ids.push_back(locationId);
 			std::list<Signature*> signatures;
-			_dbDriver->loadSignatures(ids, signatures);
+			std::set<int> loadedFromTrash;
+			_dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
 			if(signatures.size())
 			{
 				Signature * sTmp = signatures.front();
@@ -2587,10 +2605,19 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 					_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
 				}
 				r = *sTmp;
-				this->moveToTrash(s);
+				if(loadedFromTrash.size())
+				{
+					//put it back to trash
+					_dbDriver->asyncSave(sTmp);
+				}
+				else
+				{
+					delete sTmp;
+				}
 			}
 		}
 	}
+	UDEBUG("");

 	if(uncompressedData && r.getImageRaw().empty() && r.getImage().size())
 	{
@@ -2604,20 +2631,10 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 		}
 		else
 		{
-			util3d::CompressionThread ctImage(r.getImage(), true);
-			util3d::CompressionThread ctDepth(r.getDepth(), true);
-			util3d::CompressionThread ctDepth2D(r.getDepth2D(), false);
-			ctImage.start();
-			ctDepth.start();
-			ctDepth2D.start();
-			ctImage.join();
-			ctDepth.join();
-			ctDepth2D.join();
-			r.setImageRaw(ctImage.getUncompressedData());
-			r.setDepthRaw(ctDepth.getUncompressedData());
-			r.setDepth2DRaw(ctDepth2D.getUncompressedData());
+			r.uncompressData();
 		}
 	}
+	UDEBUG("");

 	return r;
 }
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -84,10 +84,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 }

-void Odometry::reset()
+void Odometry::reset(const Transform & initialPose)
 {
 	_resetCurrentCount = 0;
-	_pose = Transform::getIdentity();
+	_pose = initialPose;
 }

 bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -193,7 +193,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	}

 	_memory = new Memory(customParameters);
-	if(!_memory->init("", false, ParametersMap(), false))
+	if(!_memory->init("", false, ParametersMap()))
 	{
 		UERROR("Error initializing the memory for BOW Odometry.");
 	}
@@ -205,10 +205,10 @@ OdometryBOW::~OdometryBOW()
 }


-void OdometryBOW::reset()
+void OdometryBOW::reset(const Transform & initialPose)
 {
-	Odometry::reset();
-	_memory->init("", false, ParametersMap(), false);
+	Odometry::reset(initialPose);
+	_memory->init("", false, ParametersMap());
 	localMap_.clear();
 }

@@ -471,9 +471,9 @@ OdometryOpticalFlow::~OdometryOpticalFlow()
 }


-void OdometryOpticalFlow::reset()
+void OdometryOpticalFlow::reset(const Transform & initialPose)
 {
-	Odometry::reset();
+	Odometry::reset(initialPose);
 	lastFrame_ = cv::Mat();
 	lastCorners_.clear();
 	lastCorners3D_->clear();
@@ -1063,9 +1063,9 @@ OdometryICP::OdometryICP(int decimation,
 {
 }

-void OdometryICP::reset()
+void OdometryICP::reset(const Transform & initialPose)
 {
-	Odometry::reset();
+	Odometry::reset(initialPose);
 	_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
 	_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
 }
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -65,11 +65,6 @@ std::string Parameters::getDefaultWorkingDirectory()
 	return path;
 }

-std::string Parameters::getDefaultDatabasePath()
-{
-	return getDefaultWorkingDirectory() + getDefaultDatabaseName();
-}
-
 std::string Parameters::getDefaultDatabaseName()
 {
 	return "rtabmap.db";
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -246,31 +246,25 @@ void Rtabmap::flushStatisticLogs()
 	}
 }

-void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
+void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
 {
-	if(deleteMemory)
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
 	{
-		// Set the working directory and database path before
-		ParametersMap::const_iterator iter;
-		if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
-		{
-			this->setWorkingDirectory(iter->second.c_str());
-		}
-		if((iter=parameters.find(Parameters::kRtabmapDatabasePath())) != parameters.end())
-		{
-			this->setDatabasePath(iter->second.c_str());
-		}
-		Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
-		Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
-		this->resetMemory(true);
+		this->setWorkingDirectory(iter->second.c_str());
 	}

+	_databasePath = databasePath;
+	if(_databasePath.empty())
+	{
+		_databasePath = _wDir + Parameters::getDefaultDatabaseName();
+	}
+	UINFO("Using database \"%s\".", _databasePath.c_str());
 	this->parseParameters(parameters);
-
 	setupLogFiles();
 }

-void Rtabmap::init(const std::string & configFile, bool deleteMemory)
+void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
 {
 	// fill ctrl struct with values from the configuration file
 	ParametersMap param;// = Parameters::defaultParameters;
@@ -281,7 +275,7 @@ void Rtabmap::init(const std::string & configFile, bool deleteMemory)
 		this->readParameters(configFile, param);
 	}

-	this->init(param, deleteMemory);
+	this->init(param, databasePath);
 }

 void Rtabmap::close()
@@ -313,20 +307,23 @@ void Rtabmap::close()
 		delete _bayesFilter;
 		_bayesFilter = 0;
 	}
+	_databasePath.clear();
 }

 void Rtabmap::parseParameters(const ParametersMap & parameters)
 {
+	if(_databasePath.empty())
+	{
+		UERROR("RTAB-Map is not initialized. Call rtabmap::init() instead.");
+		return;
+	}
+
 	ULOGGER_DEBUG("");
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
 	{
 		this->setWorkingDirectory(iter->second.c_str());
 	}
-	if((iter=parameters.find(Parameters::kRtabmapDatabasePath())) != parameters.end())
-	{
-		this->setDatabasePath(iter->second.c_str());
-	}

 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
@@ -380,7 +377,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	if(!_memory)
 	{
 		_memory = new Memory(parameters);
-		_memory->init(getDatabasePath(), false, parameters);
+		_memory->init(_databasePath, false, parameters, true);

 		//generate map
 		if(_rgbdSlamMode && _memory->getLastWorkingSignature())
@@ -429,11 +426,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 }

-std::string Rtabmap::getDatabasePath() const
-{
-	return _databasePath.empty()?_wDir + Parameters::getDefaultDatabaseName():_databasePath;
-}
-
 int Rtabmap::getLoopClosureId() const
 {
 	return _lcHypothesisId;
@@ -647,7 +639,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 	}
 }

-void Rtabmap::resetMemory(bool dbOverwritten)
+void Rtabmap::resetMemory()
 {
 	_retrievedId = 0;
 	_lcHypothesisValue = 0;
@@ -660,7 +652,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)

 	if(_memory)
 	{
-		_memory->init(getDatabasePath(), dbOverwritten);
+		_memory->init(_databasePath, true, ParametersMap(), true);
 		if(_memory->getLastWorkingSignature())
 		{
 			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
@@ -670,13 +662,11 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 			_bayesFilter->reset();
 		}
 	}
-	else if(dbOverwritten)
+	else
 	{
-		// May be memory should be already created here, and use init above...
-		UINFO("Erasing file : \"%s\"", getDatabasePath().c_str());
-		UFile::erase(getDatabasePath());
+		UERROR("RTAB-Map is not initialized. No memory to reset...");
 	}
-	this->setupLogFiles(dbOverwritten);
+	this->setupLogFiles(true);
 }

 //============================================================
@@ -1803,28 +1793,8 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }

-void Rtabmap::setDatabasePath(const std::string & path)
-{
-	if(!path.empty())
-	{
-		ULOGGER_DEBUG("Comparing new database path \"%s\" with \"%s\"", path.c_str(), _databasePath.c_str());
-		if(path.compare(_databasePath) != 0)
-		{
-			UDEBUG("Set new database path to \"%s\"", _databasePath.c_str());
-			_databasePath = path;
-			if(_memory)
-			{
-				UDEBUG("Reset memory!");
-				this->resetMemory();
-			}
-		}
-	}
-	else
-	{
-		ULOGGER_ERROR("Cannot set null database path!");
-	}
-}
-
+
+
 void Rtabmap::deleteLocation(int locationId)
 {
 	if(_memory)
@@ -2238,10 +2208,12 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,

 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
+			UDEBUG("");
 			Signature data = _memory->getSignatureData(*iter);
+			UDEBUG("");
 			if(data.id() != Memory::kIdInvalid)
 			{
-				signatures.insert(std::make_pair(*iter, data));
+				signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
 				mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 			}
 		}
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -168,6 +168,10 @@ void RtabmapThread::mainLoop()
 	case kStateDetecting:
 		this->process();
 		break;
+	case kStateInit:
+		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
+		_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
+		break;
 	case kStateChangingParameters:
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
@@ -179,6 +183,14 @@ void RtabmapThread::mainLoop()
 		_rtabmap->resetMemory();
 		this->clearBufferedData();
 		break;
+	case kStateClose:
+		if(_dataBuffer.size())
+		{
+			UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
+			this->clearBufferedData();
+		}
+		_rtabmap->close();
+		break;
 	case kStateDumpingMemory:
 		_rtabmap->dumpData();
 		break;
@@ -197,14 +209,6 @@ void RtabmapThread::mainLoop()
 	case kStateGeneratingTOROGraphGlobal:
 		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
-	case kStateDeletingMemory:
-		if(!parameters.at("path").empty())
-		{
-			_rtabmap->setDatabasePath(parameters.at("path"));
-		}
-		_rtabmap->resetMemory(true);
-		this->clearBufferedData();
-		break;
 	case kStateCleanDataBuffer:
 		this->clearBufferedData();
 		break;
@@ -254,7 +258,20 @@ void RtabmapThread::handleEvent(UEvent* event)
 	{
 		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
 		RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
-		if(cmd == RtabmapEventCmd::kCmdResetMemory)
+		if(cmd == RtabmapEventCmd::kCmdInit)
+		{
+			ULOGGER_DEBUG("CMD_INIT");
+			ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
+			UASSERT(!rtabmapEvent->getStr().empty());
+			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
+			pushNewState(kStateInit, parameters);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdClose)
+		{
+			ULOGGER_DEBUG("CMD_CLOSE");
+			pushNewState(kStateClose);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdResetMemory)
 		{
 			ULOGGER_DEBUG("CMD_RESET_MEMORY");
 			pushNewState(kStateReseting);
@@ -313,13 +330,6 @@ void RtabmapThread::handleEvent(UEvent* event)
 			pushNewState(kStateGeneratingTOROGraphGlobal, param);

 		}
-		else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
-		{
-			ULOGGER_DEBUG("CMD_DELETE_MEMORY");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			pushNewState(kStateDeletingMemory, param);
-		}
 		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
 		{
 			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
@@ -384,12 +394,19 @@ void RtabmapThread::process()
 	getData(data);
 	if(data.isValid())
 	{
-		_rtabmap->process(data);
+		if(_rtabmap->getMemory())
+		{
+			_rtabmap->process(data);

-		Statistics stats = _rtabmap->getStatistics();
-		stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
-		ULOGGER_DEBUG("posting statistics_ event...");
-		this->post(new RtabmapEvent(stats));
+			Statistics stats = _rtabmap->getStatistics();
+			stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
+			ULOGGER_DEBUG("posting statistics_ event...");
+			this->post(new RtabmapEvent(stats));
+		}
+		else
+		{
+			UERROR("RTAB-Map is not initialized! Ignoring received data...");
+		}
 	}
 }

--- a/corelib/src/Signature.cpp
+++ b/corelib/src/Signature.cpp
@@ -88,7 +88,7 @@ Signature::Signature(

 Signature::~Signature()
 {
-	ULOGGER_DEBUG("id=%d", _id);
+	//ULOGGER_DEBUG("id=%d", _id);
 }

 void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
@@ -247,9 +247,9 @@ void Signature::uncompressData()
 	if(_imageRaw.empty() && _image.size())
 	{
 		//uncompress data
-		util3d::CompressionThread ctImage(_image, true);
-		util3d::CompressionThread ctDepth(_depth, true);
-		util3d::CompressionThread ctDepth2D(_depth2D, false);
+		util3d::CompressionThread ctImage(&_image, true);
+		util3d::CompressionThread ctDepth(&_depth, true);
+		util3d::CompressionThread ctDepth2D(&_depth2D, false);
 		ctImage.start();
 		ctDepth.start();
 		ctDepth2D.start();
@@ -262,4 +262,55 @@ void Signature::uncompressData()
 	}
 }

+void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const
+{
+	if(image)
+	{
+		*image = _imageRaw;
+	}
+	if(depth)
+	{
+		*depth = _depthRaw;
+	}
+	if(depth2D)
+	{
+		*depth2D = _depth2DRaw;
+	}
+	if( (image && image->empty()) ||
+		(depth && depth->empty()) ||
+		(depth2D && depth2D->empty()))
+	{
+		util3d::CompressionThread ctImage(&_image, true);
+		util3d::CompressionThread ctDepth(&_depth, true);
+		util3d::CompressionThread ctDepth2D(&_depth2D, false);
+		if(image && image->empty())
+		{
+			ctImage.start();
+		}
+		if(depth && depth->empty())
+		{
+			ctDepth.start();
+		}
+		if(depth2D && depth2D->empty())
+		{
+			ctDepth2D.start();
+		}
+		ctImage.join();
+		ctDepth.join();
+		ctDepth2D.join();
+		if(image)
+		{
+			*image = ctImage.getUncompressedData();
+		}
+		if(depth)
+		{
+			*depth = ctDepth.getUncompressedData();
+		}
+		if(depth2D)
+		{
+			*depth2D = ctDepth2D.getUncompressedData();
+		}
+	}
+}
+
 } //namespace rtabmap
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -66,6 +66,7 @@ namespace util3d

 // format : ".png" ".jpg" "" (empty is general)
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
+	constCompressedData_(0),
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
@@ -74,8 +75,8 @@ CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & fo
 	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
 }
 // assume image
-CompressionThread::CompressionThread(const std::vector<unsigned char> & bytes, bool isImage) :
-	compressedData_(bytes),
+CompressionThread::CompressionThread(const std::vector<unsigned char> * bytes, bool isImage) :
+	constCompressedData_(bytes),
 	image_(isImage),
 	compressMode_(false)
 {}
@@ -97,15 +98,15 @@ void CompressionThread::mainLoop()
 	}
 	else // uncompress
 	{
-		if(!compressedData_.empty())
+		if(constCompressedData_ && constCompressedData_->size())
 		{
 			if(image_)
 			{
-				uncompressedData_ = uncompressImage(compressedData_);
+				uncompressedData_ = uncompressImage(*constCompressedData_);
 			}
 			else
 			{
-				uncompressedData_ = uncompressData(compressedData_);
+				uncompressedData_ = uncompressData(*constCompressedData_);
 			}
 		}
 	}