-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-01 17:10:26 +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/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_);
 			}
 		}
 	}