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

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;}
@@ -396,6 +397,8 @@ private:
 	MarkerDetector * _markerDetector;
 
 	GlobalDescriptorExtractor * _globalDescriptorExtractor;
+
+	bool _dummyDictionary;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -264,6 +264,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
     RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. Ignored on initialization if %s is enabled.", kMemIncrementalMemory().c_str(), kMemInitWMWithAllNodes().c_str()).c_str());
     RTABMAP_PARAM(Kp, SerializeWithChecksum,    bool, true,   "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built.");
+    RTABMAP_PARAM(Kp, AutoUpdate,               bool, true,   "Automatically update on any parameter changes affecting the structure of the dictionary. Otherwise, it is updated only when triggered externally (i.e., deferring initialization time).");
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");

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

corelib/include/rtabmap/core/VWDictionary.h

@@ -108,6 +108,8 @@ public:
 	void setIncrementalDictionary();
 	void setFixedDictionary(const std::string & dictionaryPath);
 	bool isModified() const;
+	void setAutoUpdate(bool enabled) {_autoUpdate = enabled;} // Enable/Disable internal update when parameters change, update() has to be externally triggered if disabled.
+	bool isAutoUpdateEnabled() const {return _autoUpdate;}
 
 	std::vector<unsigned char> serializeIndex() const;
 	void deserializeIndex(const std::vector<unsigned char> & data);
@@ -137,6 +139,7 @@ private:
 	bool _incrementalFlann;
 	float _rebalancingFactor;
 	bool _byteToFloat;
+	bool _autoUpdate;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)

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,11 +412,23 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			{
 				if(wordIds.size())
 				{
-					std::list<VisualWord*> words;
-					_dbDriver->loadWords(wordIds, words);
-					for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
+					if(_dummyDictionary)
 					{
-						_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;
@@ -435,7 +448,12 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			_dbDriver->load(*_vwd, _vwd->isIncremental());
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
-		_vwd->update();
+		if(_vwd->isAutoUpdateEnabled())	{
+			_vwd->update();
+		}
+		else {
+			UDEBUG("Dictionary update skipped (%s=true)", Parameters::kKpAutoUpdate().c_str());
+		}
 		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...")));
@@ -561,7 +579,12 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				UWARN("%s", msg.c_str());
 				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
 				_memoryChanged = true; // This will force rtabmap to save back the dictionary even if we don't process any new data
-				_vwd->update();
+				if(_vwd->isAutoUpdateEnabled())	{
+					_vwd->update();
+				}
+				else {
+					UDEBUG("Dictionary update skipped (%s=true)", Parameters::kKpAutoUpdate().c_str());
+				}
 			}
 		}
 
@@ -595,6 +618,22 @@ 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;
+	_vwd->setAutoUpdate(!_dummyDictionary);
+}
+
 void Memory::saveFlannIndex(bool postInitClosingEvents)
 {
 	if(!_dbDriver) {
@@ -1455,10 +1494,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 +3171,13 @@ 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;
 	}
 }
@@ -5093,6 +5132,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

@@ -6934,6 +6934,16 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
 	}
 }
 
+void Rtabmap::setDummyDictionary(bool enabled)
+{
+	if(_memory) {
+		_memory->setDummyDictionary(enabled);
+	}
+	else {
+		UERROR("Memory is null, cannot set dummy dictionary.");
+	}
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);

corelib/src/VWDictionary.cpp

@@ -73,7 +73,8 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	useDistanceL1_(false),
 	_flannIndex(new FlannIndex()),
 	_modified(true),
-	_strategy(kNNBruteForce)
+	_strategy(kNNBruteForce),
+	_autoUpdate(true)
 {
 	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
 	this->parseParameters(parameters);
@@ -108,7 +109,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())
 	{
@@ -121,7 +122,12 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
-		this->update();
+		if(_autoUpdate) {
+			this->update();
+		}
+		else {
+			UDEBUG("Dictionary update skipped (%s=true)", Parameters::kKpAutoUpdate().c_str());
+		}
 	}
 
 	if(incrementalDictionary)
@@ -266,7 +272,12 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 				_dictionaryPath = dictionaryPath;
 				_newDictionaryPath = dictionaryPath;
 				_incrementalDictionary = false;
-				this->update();
+				if(_autoUpdate) {
+					this->update();
+				}
+				else {
+					UDEBUG("Dictionary update skipped (%s=true)", Parameters::kKpAutoUpdate().c_str());
+				}
 				UWARN("Loaded %d words!", (int)_visualWords.size());
 			}
 		}
@@ -345,7 +356,12 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
-		this->update();
+		if(_autoUpdate)	{
+			this->update();
+		}
+		else {
+			UDEBUG("Dictionary update skipped (%s=true)", Parameters::kKpAutoUpdate().c_str());
+		}
 		return true;
 	}
 	return false;

tools/DetectMoreLoopClosures/main.cpp

@@ -198,6 +198,7 @@ int main(int argc, char * argv[])
 	// Add some optimizations (soft set, can be overriden by arguments)
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
 	inputParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // should be incremental to update links
+	inputParams.insert(ParametersPair(Parameters::kKpAutoUpdate(), "false")); // don't build the dictionary (don't need it) 
 
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
@@ -249,6 +250,11 @@ 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());
 

tools/ReduceGraph/main.cpp

@@ -57,11 +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"
+			"                   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 from WM and LTM. This assumes that the original global 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 reduction from WM and LTM. Warning: this could remove completly unconnected graphes from WM and LTM. Usage of --remove_orphan_nodes is safer. Backup you database before trying this.\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);
@@ -146,6 +154,7 @@ int main(int argc, char * argv[])
 	ParametersMap inputParams;
 	inputParams.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); // load the whole map in RAM
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
+	inputParams.insert(ParametersPair(Parameters::kKpAutoUpdate(), "false")); // don't build the dictionary (don't need it) 
 
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
@@ -159,9 +168,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
@@ -170,7 +181,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;
@@ -205,9 +222,36 @@ int main(int argc, char * argv[])
 			constraints,
 			posesOut,
 			linksOut);
-		isWholeGraphConnected = posesOut.size() == poses.size();	
+		isWholeGraphConnected = posesOut.size() == poses.size();
 
-		printf("Whole global graph is%s connected to all nodes of WM/LTM (%ld/%ld)\n", isWholeGraphConnected?"":" not", posesOut.size(), ids.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;
@@ -224,6 +268,7 @@ int main(int argc, char * argv[])
 		vids.insert(vids.end(), ids.rbegin(), ids.rend());
 	}
 
+	int totalLinksRemoved = 0;
 	if(preCleanup)
 	{
 		if(memory.getMaxStMemSize() <= 1)
@@ -232,7 +277,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);
@@ -243,12 +287,12 @@ 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());
 		}
 	}
 
@@ -271,6 +315,12 @@ 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;
+	}
 
 	if(isWholeGraphConnected || removeAllOrphanNodes)
 	{
@@ -308,8 +358,14 @@ int main(int argc, char * argv[])
 				{
 					if(posesOut.find(iter->first) == posesOut.end())
 					{
-						memory.deleteLocation(iter->first, 0);
-						printf("Removed %d from WM/LTM.\n", iter->first);
+						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);
 					}
 				}
 			}
@@ -377,6 +433,33 @@ 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.deleteLocation(id, 0, /*keepLinkedInDb*/ true);
+			++transferred;
+		}
+	}
+	printf("Restoring Working Memory... done! Transferred %d nodes.\n", transferred);
 	
 	printf("Saving all changes to database...\n");
 	memory.close(true);