Reduce graph: added option to remove orphan nodes from WM after reduction (#1735)

* Reduce graph: remove orphan nodes from WM after reduction

* fixed eror

* ignore ids< 0

* add warning and cleanup only when option is used

* updated feedback

* Abort --sync_wm_and_opt_graph if a lot more WM nodes have to be transferred.

* typo

* refactored... completly

* fixed optimized graph cleared

* updated option description

* updated comment

* Added dummy dictionary function to speedup initialization when we dont need the dictionary fully loaded in memory.

* Fixed nodes weight not modified in db when deleted

* Removed AutoUpdate parameter, not needed

* Fixed dummy dictionary usage for detectMoreLoopClosures. Added checks to disable graph reduction when intermediate nodes are detected.

* updated log

corelib/include/rtabmap/core/DBDriver.h

@@ -162,7 +162,7 @@ public:
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary & dictionary, bool lastStateOnly = true) const;
+	void load(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const;
 	void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
 	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage
@@ -279,7 +279,7 @@ protected:
 	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const = 0;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0;
+	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -138,7 +138,7 @@ protected:
 	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const;
+	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;

corelib/include/rtabmap/core/Memory.h

@@ -141,11 +141,12 @@ public:
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
 	void convertToIntermediate(int locationId);
-	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void deleteLocation(int locationId, std::list<int> * deletedWords = 0, bool keepLinkedInDb = false);
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
+	void setDummyDictionary(bool enabled);
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -279,7 +280,6 @@ private:
 	std::list<Signature *> getRemovableSignatures(int count,
 			const std::set<int> & ignoredIds = std::set<int>());
 	int getNextId();
-	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
 	bool canBeReduced(const Link & link, float maxDistance, int direction);
@@ -396,6 +396,8 @@ private:
 	MarkerDetector * _markerDetector;
 
 	GlobalDescriptorExtractor * _globalDescriptorExtractor;
+
+	bool _dummyDictionary;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Rtabmap.h

@@ -229,6 +229,7 @@ public:
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
 	void addNodesToRepublish(const std::vector<int> & ids);
+	void setDummyDictionary(bool enabled);
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -402,6 +403,8 @@ private:
 	int _pathStuckCount;
 	float _pathStuckDistance;
 
+	bool _dummyDictionary;
+
 #ifdef RTABMAP_PYTHON
 	PythonInterface * _python;
 #endif

corelib/include/rtabmap/core/Signature.h

@@ -70,10 +70,10 @@ public:
 	int id() const {return _id;}
 	int mapId() const {return _mapId;}
 
-	void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
+	void setWeight(int weight) {_modified=_modified || _weight!=weight;_weight = weight;}
 	int getWeight() const {return _weight;}
 
-	void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
+	void setLabel(const std::string & label) {_modified=_modified || _label.compare(label)!=0;_label = label;}
 	const std::string & getLabel() const {return _label;}
 
 	double getStamp() const {return _stamp;}

corelib/src/DBDriver.cpp

@@ -539,10 +539,10 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
+void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadQuery(dictionary, lastStateOnly);
+	this->loadQuery(dictionary, lastStateOnly, idsOnly);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -3538,7 +3538,7 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool lo
 	}
 }
 
-void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const
+void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const
 {
 	ULOGGER_DEBUG("");
 	if(_ppDb)
@@ -3552,7 +3552,12 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
 		std::list<VisualWord *> visualWords;
 
 		// Get the visual words
-		query << "SELECT id, descriptor_size, descriptor FROM Word ";
+		query << "SELECT id";
+		if(!idsOnly)
+		{
+			query << ", descriptor_size, descriptor";
+		}
+		query << " FROM Word ";
 		if(lastStateOnly)
 		{
 			if(uStrNumCmp(_version, "0.11.11") >= 0)
@@ -3581,27 +3586,29 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
 			int index=0;
 			id = sqlite3_column_int(ppStmt, index++); 			// VisualWord Id
 
-			descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-			descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-			dRealSize = sqlite3_column_bytes(ppStmt, index++);
-
 			cv::Mat d;
-			if(dRealSize == descriptorSize)
+			if(!idsOnly) {
-			{
+				descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-				// CV_8U binary descriptors
+				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-				d = cv::Mat(1, descriptorSize, CV_8U);
+				dRealSize = sqlite3_column_bytes(ppStmt, index++);
-			}
-			else if(dRealSize/int(sizeof(float)) == descriptorSize)
-			{
-				// CV_32F
-				d = cv::Mat(1, descriptorSize, CV_32F);
-			}
-			else
-			{
-				UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
-			}
 
-			memcpy(d.data, descriptor, dRealSize);
+				if(dRealSize == descriptorSize)
+				{
+					// CV_8U binary descriptors
+					d = cv::Mat(1, descriptorSize, CV_8U);
+				}
+				else if(dRealSize/int(sizeof(float)) == descriptorSize)
+				{
+					// CV_32F
+					d = cv::Mat(1, descriptorSize, CV_32F);
+				}
+				else
+				{
+					UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
+				}
+
+				memcpy(d.data, descriptor, dRealSize);
+			}
 			VisualWord * vw = new VisualWord(id, d);
 			vw->setSaved(true);
 			dictionary.addWord(vw);
@@ -3621,7 +3628,7 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
 		getLastWordId(id);
 		dictionary.setLastWordId(id);
 
-		if(uStrNumCmp(_version, "0.23.0") >= 0) {
+		if(!idsOnly && uStrNumCmp(_version, "0.23.0") >= 0) {
 			// load dictionary index
 			std::stringstream query3;
 			query3 << "SELECT dictionary_index "

corelib/src/Memory.cpp

@@ -138,7 +138,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
 	_parallelized(Parameters::defaultKpParallelized()),
-	_registrationVis(0)
+	_registrationVis(0),
+	_dummyDictionary(false)
 {
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
@@ -411,31 +412,55 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			{
 				if(wordIds.size())
 				{
-					std::list<VisualWord*> words;
+					if(_dummyDictionary)
-					_dbDriver->loadWords(wordIds, words);
-					for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
 					{
-						_vwd->addWord(*iter);
+						for(std::set<int>::iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
+						{
+							VisualWord * w  = new VisualWord(*iter, cv::Mat());
+							w->setSaved(true);
+							_vwd->addWord(w); // placeholder descriptor
+						}
+					}
+					else
+					{
+						std::list<VisualWord*> words;
+						_dbDriver->loadWords(wordIds, words);
+						for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
+						{
+							_vwd->addWord(*iter);
+						}
 					}
 					// Get Last word id
 					int id = 0;
 					_dbDriver->getLastWordId(id);
 					_vwd->setLastWordId(id);
 				}
+				else {
+					_dummyDictionary = false;
+				}
 			}
 			else
 			{
-				_dbDriver->load(*_vwd, false);
+				_dbDriver->load(*_vwd, false, _dummyDictionary);
 			}
 		}
 		else
 		{
 			UDEBUG("load words");
 			// load the last dictionary
-			_dbDriver->load(*_vwd, _vwd->isIncremental());
+			_dbDriver->load(*_vwd, _vwd->isIncremental(), _dummyDictionary);
+		}
+		UDEBUG("%d words loaded! (type=%s, dim=%d)",
+			_vwd->getUnusedWordsSize(),
+			_vwd->getVisualWords().empty()?"NA":_vwd->getVisualWords().begin()->second->getDescriptor().empty()?"dummy":_vwd->getVisualWords().begin()->second->getDescriptor().type() == CV_32FC1?"float":"binary",
+			_vwd->getVisualWords().empty()?0:_vwd->getVisualWords().begin()->second->getDescriptor().cols);
+		UDEBUG("Dictionary memory usage: %ld Bytes (%ld MB)", _vwd->getMemoryUsed(), _vwd->getMemoryUsed()/(1024*1024));
+		if(!_dummyDictionary)	{
+			_vwd->update();
+		}
+		else {
+			UDEBUG("Dictionary update skipped (dummy dictionary is enabled)");
 		}
-		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
-		_vwd->update();
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
 
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
@@ -496,6 +521,24 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				UWARN("%s", msg.c_str());
 				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
 
+				if(_dummyDictionary)
+				{
+					UWARN("Dummy dictionary cannot be used when repairing the dictionary, disabling dummy dictionary.");
+					for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+					{
+						Signature * s = this->_getSignature(i->first);
+						UASSERT(s != 0);
+						if(!s->isEnabled())
+						{
+							break;
+						}
+						this->disableWordsRef(s->id());
+					}
+					_vwd->deleteUnusedWords();
+					_vwd->clear();
+					_dummyDictionary = false;
+				}
+
 				//remove all words ref
 
 				const std::map<int, VisualWord *> & addedWords = _vwd->getVisualWords();
@@ -595,6 +638,21 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 	UDEBUG("map ids start with %d", _idMapCount);
 }
 
+void Memory::setDummyDictionary(bool enabled)
+{
+	if(_dbDriver != 0) {
+		UERROR("Dummy dictionary can only be set if the memory is not yet initialized. Ignoring.");
+		return;
+	}
+	if(enabled) {
+		UINFO("Dummy dictionary enabled.");
+	}
+	else {
+		UINFO("Dummy dictionary disabled.");
+	}
+	_dummyDictionary = enabled;
+}
+
 void Memory::saveFlannIndex(bool postInitClosingEvents)
 {
 	if(!_dbDriver) {
@@ -1324,6 +1382,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
 		UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
 		return 0;
 	}
+	else if(s->getWeight() == -1)
+	{
+		UWARN("Cannot reduce intermediate node %d (not supported).", id);
+		return 0;
+	}
 
 	if(!s->getLabel().empty())
 	{
@@ -1340,6 +1403,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
 		{
 			float distance = iter->second.transform().getNorm();
 			reducedTo = iter->second.to();
+			if(this->_getSignature(reducedTo) == 0)
+			{
+				UWARN("Node %d is not in WM/STM, cannot reduce %d to it.", reducedTo, id);
+				return 0;
+			}
 			UDEBUG("Reduce %d to %d (distance=%f)",
 				s->id(), iter->second.to(), distance);
 		}
@@ -1347,6 +1415,18 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
 		if(iter->second.type() == Link::kNeighbor)
 		{
 			neighbors.insert(*iter);
+			// neighbors should not be intermediate nodes
+			Signature * sTo = this->_getSignature(iter->first);
+			if(sTo == 0)
+			{
+				UWARN("Neighbor node %d is not in WM/STM, cannot reduce %d.", iter->first, id);
+				return 0;
+			}
+			else if(sTo->getWeight() == -1)
+			{
+				UWARN("Neighbor node %d is an intermediate node (not supported), cannot reduce %d.", iter->first, id);
+				return 0;
+			}
 		}
 	}
 	if(reducedTo>0)
@@ -1455,10 +1535,10 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
 		else
 		{
 			std::multimap<int, Link> links = s->getLinks();
-			// Setting true to make sure we save all visual
+			// Setting keepLinkedInDb=true to make sure we save all visual
 			// words that could be referenced in a previously
 			// transferred node in LTM (#979)
-			reducedId = reduceNode(s->id(), 0, true);
+			reducedId = reduceNode(s->id(), 0, /*keepLinkedInDb*/ true);
 			if(reducedToOut) {
 				*reducedToOut = reducedId;
 			}
@@ -3132,13 +3212,18 @@ void Memory::convertToIntermediate(int locationId)
 	}
 }
 
-void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
+void Memory::deleteLocation(int locationId, std::list<int> * deletedWords, bool keepLinkedInDb)
 {
-	UDEBUG("Deleting location %d", locationId);
+	UDEBUG("Deleting location %d (keepLinkedInDb=%s)", locationId, keepLinkedInDb?"true":"false");
 	Signature * location = _getSignature(locationId);
 	if(location)
 	{
-		this->moveToTrash(location, false, deletedWords);
+		this->moveToTrash(location, keepLinkedInDb, deletedWords);
+		_memoryChanged = true;
+	}
+	else
+	{
+		UWARN("Location %d has not been found in STM/WM, cannot delete it.", locationId);
 	}
 }
 
@@ -5092,6 +5177,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
 						CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
+	UASSERT_MSG(!_dummyDictionary, "Memory::createSignature() cannot be called if the memory has been initialized with a dummy dictionary.");
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().empty() &&

corelib/src/Rtabmap.cpp

@@ -180,7 +180,8 @@ Rtabmap::Rtabmap() :
 	_pathGoalIndex(0),
 	_pathTransformToGoal(Transform::getIdentity()),
 	_pathStuckCount(0),
-	_pathStuckDistance(0.0f)
+	_pathStuckDistance(0.0f),
+	_dummyDictionary(false)
 #ifdef RTABMAP_PYTHON
 	,_python(new PythonInterface())
 #endif
@@ -360,6 +361,10 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	if(!_memory)
 	{
 		_memory = new Memory(allParameters);
+		if(_dummyDictionary)
+		{
+			_memory->setDummyDictionary(true);
+		}
 		_memory->init(_databasePath, false, allParameters, true);
 	}
 
@@ -6934,6 +6939,18 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
 	}
 }
 
+void Rtabmap::setDummyDictionary(bool enabled)
+{
+	if(_memory) {
+		UERROR("Memory is already initialized, cannot set dummy dictionary. This "
+			"function can only be called after Rtabmap object is created, but "
+			"before init() is called.");
+	}
+	else {
+		_dummyDictionary = true;
+	}
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);

corelib/src/VWDictionary.cpp

@@ -108,7 +108,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 		incrementalDictionary = uStr2Bool((*iter).second.c_str());
 	}
 
-	// Verifying hypotheses strategy
+	// Verifying NN strategy
 	bool treeUpdated = false;
 	if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
 	{

tools/DetectMoreLoopClosures/main.cpp

@@ -249,9 +249,15 @@ int main(int argc, char * argv[])
 	UTimer timer;
 	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
+
+	// This avoids to load original descriptors in the dictionary
+	// to save RAM and intialization time (we don't need the dictionary for this tool)
+	rtabmap.setDummyDictionary(true); // should be set before Rtabmap::init()
+	
 	rtabmap.init(parameters, dbPath);
 	printf("Initialization... done! (%f sec)\n", timer.ticks());
 
+	// detectMoreLoopClosures would clear the optimized map if loop closures are detected
 	float xMin, yMin, cellSize;
 	bool haveOptimizedMap = !rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize).empty();
 
@@ -311,7 +317,7 @@ int main(int argc, char * argv[])
 	// Restore original parameters before saving back the database
 	rtabmap.parseParameters(originalParameters);
 
-	rtabmap.close();
+	rtabmap.close(detected>0);
 
-	return 0;
+	return detected>=0;
 }

tools/ReduceGraph/main.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Optimizer.h>
 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/global_map/OccupancyGrid.h>
@@ -56,9 +57,19 @@ void showUsage(const char * exec)
 			"%s [Options] database.db\n"
 			"Options:\n"
 			"    --keep_latest  Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
-			"    --keep_linked  Keep reduced nodes linked to graph.\n"
+			"    --keep_linked  Keep reduced nodes linked to graph (by only child->parent link)\n"
-			"    --pre_cleanup  Remove all user loop closures linking nodes closer than %s in the graph before reducing the graph.\n"
+			"                   instead of invalidating them. When --remove_orphan_nodes is used,\n"
+			"                   orphan nodes are simply transfered to LTM instead of being invalidated.\n"
+			"    --pre_cleanup  Remove all user loop closures linking nodes closer than %s nodes in the graph before reducing the graph.\n"
 			"    --radius #.#   Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n"
+			"    --remove_orphan_nodes  Remove all orphan nodes created by graph reduction \n"
+			"                           from WM and LTM. This assumes that the original global \n"
+			"                           graph connected all nodes in WM and LTM, otherwise it is skipped.\n"
+			"    --remove_all_orphan_nodes Remove all orphan nodes created or not by graph \n"
+			"                              reduction from WM and LTM. Warning: this could remove \n"
+			"                              completly unconnected graphes from WM and LTM. Usage of \n"
+			"                              --remove_orphan_nodes is safer. Backup your database \n"
+			"                              before trying this.\n"
 			"    --udebug/--uinfo/--warn can also be used to change verbosity.\n"
 			"\n", exec, Parameters::kMemSTMSize().c_str());
 	exit(1);
@@ -78,6 +89,8 @@ int main(int argc, char * argv[])
 	bool keepLinked = false;
 	float radius = 1.0f;
 	bool preCleanup = false;
+	bool removeOrphanNodes = false;
+	bool removeAllOrphanNodes = false;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -96,6 +109,14 @@ int main(int argc, char * argv[])
 		{
 			preCleanup = true;
 		}
+		else if(std::strcmp(argv[i], "--remove_orphan_nodes") == 0)
+		{
+			removeOrphanNodes = true;
+		}
+		else if(std::strcmp(argv[i], "--remove_all_orphan_nodes") == 0)
+		{
+			removeAllOrphanNodes = true;
+		}
 		else if(std::strcmp(argv[i], "--radius") == 0)
 		{
 			++i;
@@ -118,6 +139,9 @@ int main(int argc, char * argv[])
 	printf("  keep_latest = %s\n", keepLatest?"true":"false");
 	printf("  keep_linked = %s\n", keepLinked?"true":"false");
 	printf("  pre_cleanup = %s\n", preCleanup?"true":"false");
+	printf("  remove_orphan_nodes = %s\n", removeOrphanNodes?"true":"false");
+	printf("  remove_all_orphan_nodes = %s\n", removeAllOrphanNodes?"true":"false");
+	removeOrphanNodes = removeAllOrphanNodes || removeOrphanNodes;
 
 #ifdef RTABMAP_PYTHON
 	rtabmap::PythonInterface pythonInterface;
@@ -143,9 +167,11 @@ int main(int argc, char * argv[])
 	// Get parameters
 	ParametersMap parameters;
 	DBDriver * driver = DBDriver::create();
+	std::set<int> wm;
 	if(driver->openConnection(dbPath))
 	{
 		parameters = driver->getLastParameters();
+		driver->getLastNodeIds(wm);
 		driver->closeConnection(false);
 	}
 	else
@@ -154,7 +180,13 @@ int main(int argc, char * argv[])
 	}
 	delete driver;
 
+	size_t wmOrgSize = wm.size();
 	Memory memory;
+
+	// This avoids to load original descriptors in the dictionary
+	// to save RAM and intialization time (we don't need the dictionary for this tool)
+	memory.setDummyDictionary(true); // should be set before Memory::init()
+
 	printf("Initialization...\n");
 	UTimer timer;
 	ParametersMap originalParameters = parameters;
@@ -173,10 +205,53 @@ int main(int argc, char * argv[])
 		return 1;
 	}
 
-	Transform lastLocalizationPose;
+	bool isWholeGraphConnected = false;
-	std::map<int, Transform> optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose);
+	std::shared_ptr<Optimizer> optimizer(Optimizer::create(parameters));
-	float xMin, yMin, cellSize;
+
-	bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> constraints;
+	memory.getMetricConstraints(ids, poses, constraints, false, true);
+	if(!poses.empty())
+	{
+		std::map<int, Transform> posesOut;
+		std::multimap<int, Link> linksOut;
+		optimizer->getConnectedGraph(
+			keepLatest?poses.rbegin()->first:poses.begin()->first,
+			poses,
+			constraints,
+			posesOut,
+			linksOut);
+		isWholeGraphConnected = posesOut.size() == poses.size();
+
+		if(isWholeGraphConnected)
+		{
+			printf("The whole global graph is connected to all nodes of WM/LTM (%ld/%ld).\n",
+				posesOut.size(), ids.size());
+		}
+		else {
+			//Count number of nodes not in global graph that are in WM
+			int missing = 0;
+			for(auto id:wm)
+			{
+				if(posesOut.find(id) == posesOut.end()) {
+					++missing;
+				}
+			}
+			if(missing>0)
+			{
+				printf("The whole global graph is not connected to all nodes of WM/LTM (%ld/%ld) "
+					"with some (%d) of the disconnected nodes in WM.%s\n",
+					posesOut.size(), ids.size(), missing,
+					removeAllOrphanNodes?"":" You may consider using --remove_all_orphan_nodes to remove these nodes from WM if necessary.");
+			}
+			else
+			{
+				printf("The whole global graph is not connected to all nodes of WM/LTM (%ld/%ld), "
+					"though no disconnected nodes are in WM.\n",
+					posesOut.size(), ids.size());
+			}
+		}
+	}
 
 	int totalNodesReduced = 0;
 	std::vector<int> vids;
@@ -192,6 +267,7 @@ int main(int argc, char * argv[])
 		vids.insert(vids.end(), ids.rbegin(), ids.rend());
 	}
 
+	int totalLinksRemoved = 0;
 	if(preCleanup)
 	{
 		if(memory.getMaxStMemSize() <= 1)
@@ -200,7 +276,6 @@ int main(int argc, char * argv[])
 		}
 		else
 		{
-			int totalRemoved = 0;
 			for(auto id: vids)
 			{
 				auto nids = memory.getNeighborsId(id, memory.getMaxStMemSize(), -1, true, true, true);
@@ -211,15 +286,20 @@ int main(int argc, char * argv[])
 						nids.find(link.first)!=nids.end())
 					{
 						memory.removeLink(id, link.first);
-						++totalRemoved;
+						++totalLinksRemoved;
 					}
 				}
 			}
 			printf("Removed %d user links that were linking nodes that were close in the graph (below %s=%d)\n",
-				totalRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize());
+				totalLinksRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize());
 		}
 	}
 
+	// Get local optimized graph before reduction
+	Transform lastLocalizationPose;
+	std::map<int, Transform> optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose);
+
+	// Graph reduction
 	for(auto id: vids)
 	{
 		// Nodes can be already reduced by other nodes, check if they are still there
@@ -234,6 +314,82 @@ int main(int argc, char * argv[])
 		}
 	}
 	printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size());
+	if(totalNodesReduced==0 && totalLinksRemoved==0 && !removeOrphanNodes)
+	{
+		printf("Nothing to do, exiting without updating the database.\n");
+		memory.close(false);
+		return 0;
+	}
+	memory.emptyTrash();
+	memory.joinTrashThread();
+
+	if(isWholeGraphConnected || removeAllOrphanNodes)
+	{
+		// refetch the global graph after graph reduction
+		ids = memory.getAllSignatureIds();
+
+		// Check if some nodes got disconnected from the global graph
+		size_t totalBefore = poses.size();
+		poses.clear();
+		constraints.clear();
+		memory.getMetricConstraints(ids, poses, constraints, false, true);
+		std::map<int, Transform> posesOut;
+		std::multimap<int, Link> linksOut;
+		optimizer->getConnectedGraph(
+			keepLatest?poses.rbegin()->first:poses.lower_bound(0)->first,
+			poses,
+			constraints,
+			posesOut,
+			linksOut);
+
+		isWholeGraphConnected = posesOut.size() == poses.size();
+		
+		if(!isWholeGraphConnected)
+		{
+			if(removeOrphanNodes)
+			{
+				printf("Option %s is enabled, let's cleanup WM and LTM "
+					"%ld from nodes not in the global graph anymore (%ld -> reduced to %ld -> %ld connected globally).\n",
+					removeAllOrphanNodes?"--remove_all_orphan_nodes":"--remove_orphan_nodes",
+					poses.size()-posesOut.size(),
+					totalBefore, ids.size(), posesOut.size());
+				// Not all graph is connected anymore while it was before reduction: 
+				// that means we created orphan nodes, remove them
+				int transferred = 0;
+				for(std::map<int, Transform>::iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
+				{
+					if(posesOut.find(iter->first) == posesOut.end())
+					{
+						memory.deleteLocation(iter->first, 0, keepLinked);
+						if(keepLinked) {
+							printf("Transferred %d to LTM (--keep_linked).\n", iter->first);
+						}
+						else {
+							printf("Removed %d from WM/LTM.\n", iter->first);
+						}
+						wm.erase(iter->first);
+						++transferred;
+					}
+				}
+				if(transferred>0) {
+					memory.emptyTrash();
+					memory.joinTrashThread();
+				}
+			}
+			else {
+				printf("[Warning] After graph reduction, the global graph is not all "
+					"connected anymore while it was before (%ld -> reduced to %ld -> %ld connected globally). "
+					"Add %s option to remove the %ld orphan nodes from WM and LTM.\n",
+					totalBefore, ids.size(), posesOut.size(),
+					removeAllOrphanNodes?"--remove_all_orphan_nodes":"--remove_orphan_nodes",
+					poses.size()-posesOut.size());
+			}
+		}
+		else
+		{
+			printf("Whole optimized graph is still all connected after graph reduction (%ld/%ld)\n", posesOut.size(), ids.size());
+		}
+	} // else: we cannot detect orphan nodes if the original graph was not all connected.
 
 	if(!optimizedPoses.empty())
 	{
@@ -251,9 +407,13 @@ int main(int argc, char * argv[])
 				++iter;
 			}
 		}
-		printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
+		printf("Updated local optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
+		printf("Saving back %ld optimized poses to database.\n", optimizedPoses.size());
 		memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose);
 	}
+
+	float xMin, yMin, cellSize;
+	bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
 	if(hasOptimizedMap)
 	{
 		printf("The database has a global occupancy grid, regenerating one with the remaining nodes of the optimized graph!\n");
@@ -281,6 +441,37 @@ int main(int argc, char * argv[])
 	// Restore original parameters before saving back the database
 	memory.parseParameters(originalParameters);
 
+	// Restore Working Memory (Mem/InitWMWithAllNodes is used above):
+	// When memory is closing, it updates the Info table with current time,
+	// then move to trash the nodes afterwards so that nodes's update time
+	// in the database is greater than last info entry. This is how rtabmap
+	// knows which nodes are in working memory. The idea here is the move to
+	// trash nodes that were not in original WM before closing Memory. We can
+	// use deleteLocation with keepLinkedInDb=true to achieve what Memory::clear() does.
+	ids = memory.getAllSignatureIds(); // LTM ids
+	// Count number of nodes in WM that were reduced (directly/indirectly)
+	int wmReduced = 0;
+	for(auto id:wm)
+	{
+		if(ids.find(id) == ids.end()) {
+			++wmReduced;
+		}
+	}
+	printf("Restoring Working Memory (org:%ld -> reduced:%ld)...\n", wmOrgSize, wm.size() - wmReduced);
+	int transferred = 0;
+	for(auto id:ids)
+	{
+		if(wm.find(id) == wm.end() && memory.getWorkingMem().find(id) != memory.getWorkingMem().end()) {
+			memory.deleteLocation(id, 0, /*keepLinkedInDb*/ true);
+			++transferred;
+		}
+	}
+	if(transferred>0) {
+		memory.emptyTrash();
+		memory.joinTrashThread();
+	}
+	printf("Restoring Working Memory... done! Transferred %d nodes.\n", transferred);
+	
 	printf("Saving all changes to database...\n");
 	memory.close(true);