Added statistics for incremental flann index size

corelib/include/rtabmap/core/Statistics.h

@@ -120,6 +120,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Indexed_words, words);
+	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
 	RTABMAP_STATS(Keypoint, Response_threshold,);
 
 public:

corelib/include/rtabmap/core/VWDictionary.h

@@ -75,6 +75,8 @@ public:
 	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	unsigned int getIndexedWordsCount() const;
+	unsigned int getIndexMemoryUsed() const;
 	void setNNStrategy(NNStrategy strategy);
 	bool isIncremental() const {return _incrementalDictionary;}
 	void setIncrementalDictionary();

corelib/src/Rtabmap.cpp

@@ -230,6 +230,8 @@ void Rtabmap::setupLogFiles(bool overwrite)
 			fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
 			fprintf(_foutInt, " 18-Local graph size\n");
 			fprintf(_foutInt, " 19-Sensor data id\n");
+			fprintf(_foutInt, " 20-Indexed words\n");
+			fprintf(_foutInt, " 21-Index memory usage (KB)\n");
 		}
 
 		ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str());
@@ -2270,6 +2272,8 @@ bool Rtabmap::process(
 
 			// Surf specific parameters
 			statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
+			statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount());
+			statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed());
 
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
@@ -2514,7 +2518,7 @@ bool Rtabmap::process(
 									timeLocalTimeDetection,
 									timeLocalSpaceDetection,
 									timeMapOptimization);
-		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
 									_loopClosureHypothesis.first,
 									_highestHypothesis.first,
 									(int)signaturesRemoved.size(),
@@ -2533,7 +2537,9 @@ bool Rtabmap::process(
 									rehearsalMaxId,
 									rehearsalMaxId>0?1:0,
 									localGraphSize,
-									data.id());
+									data.id(),
+									_memory->getVWDictionary()->getIndexedWordsCount(),
+									_memory->getVWDictionary()->getIndexMemoryUsed());
 		if(_statisticLogsBufferedInRAM)
 		{
 			_bufferedLogsF.push_back(logF);

corelib/src/VWDictionary.cpp

@@ -83,6 +83,41 @@ public:
 		}
 	}
 
+	unsigned int indexedFeatures() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(binaryType_)
+		{
+			return ((const flann::Index<flann::Hamming<unsigned char> >*)index_)->size();
+
+		}
+		else
+		{
+			return ((const flann::Index<flann::L2<float> >*)index_)->size();
+		}
+	}
+
+	// return KB
+	unsigned int memoryUsed() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(binaryType_)
+		{
+			return ((const flann::Index<flann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+
+		}
+		else
+		{
+			return ((const flann::Index<flann::L2<float> >*)index_)->usedMemory()/1000;
+		}
+	}
+
 	void build(
 			const cv::Mat & features,
 			const flann::IndexParams& params,
@@ -427,6 +462,16 @@ int VWDictionary::getLastIndexedWordId() const
 	}
 }
 
+unsigned int VWDictionary::getIndexedWordsCount() const
+{
+	return _flannIndex->indexedFeatures();
+}
+
+unsigned int VWDictionary::getIndexMemoryUsed() const
+{
+	return _flannIndex->memoryUsed();
+}
+
 void VWDictionary::update()
 {
 	ULOGGER_DEBUG("");
@@ -447,21 +492,27 @@ void VWDictionary::update()
 		   (_notIndexedWords.size() || _removedIndexedWords.size()) &&
 		   oldSize)
 		{
-			for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
+			if(_notIndexedWords.size())
 			{
-				VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
+				int i = _dataTree.rows;
-				UASSERT(w);
+				_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
-				UASSERT(w->getDescriptor().cols == _dataTree.cols);
+				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
-				UASSERT(w->getDescriptor().type() == _dataTree.type());
-				_dataTree.push_back(w->getDescriptor());
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(_dataTree.rows-1, w->id()));
-				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), _dataTree.rows-1));
-				if(!inserted.second)
 				{
-					//update to new index
+					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
-					inserted.first->second = _dataTree.rows-1;
+					UASSERT(w);
+					UASSERT(w->getDescriptor().cols == _dataTree.cols);
+					UASSERT(w->getDescriptor().type() == _dataTree.type());
+					_dataTree.push_back(w->getDescriptor());
+					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
+					std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
+					if(!inserted.second)
+					{
+						//update to new index
+						inserted.first->second = i;
+					}
+					_flannIndex->addPoints(w->getDescriptor());
+					++i;
 				}
-				_flannIndex->addPoints(w->getDescriptor());
 			}
 			for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
 			{
@@ -475,6 +526,8 @@ void VWDictionary::update()
 				oldSize)
 		{
 			//just add not indexed words
+			int i = _dataTree.rows;
+			_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
 			for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
 			{
 				VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -482,9 +535,10 @@ void VWDictionary::update()
 				UASSERT(w->getDescriptor().cols == _dataTree.cols);
 				UASSERT(w->getDescriptor().type() == _dataTree.type());
 				_dataTree.push_back(w->getDescriptor());
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(_dataTree.rows-1, w->id()));
+				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
-				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), _dataTree.rows-1));
+				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
 				UASSERT(inserted.second);
+				++i;
 			}
 		}
 		else

tools/ConsoleApp/main.cpp

@@ -65,6 +65,7 @@ void showUsage()
 			"  -warn                           Set Log level to Warning (Default Error)\n"
 			"  -exit_warn                      Set exit level to Warning (Default Fatal)\n"
 			"  -exit_error                     Set exit level to Error (Default Fatal)\n"
+			"  -log_console                    Log to console\n"
 			"  -v                              Get version of RTAB-Map\n"
 			"  -input \"path\"                 Load previous database if it exists.\n");
 	exit(1);
@@ -118,6 +119,7 @@ int main(int argc, char * argv[])
 	ParametersMap pm;
 	ULogger::Level logLevel = ULogger::kError;
 	ULogger::Level exitLevel = ULogger::kFatal;
+	bool logConsole = false;
 
 	for(int i=1; i<argc; ++i)
 	{
@@ -248,6 +250,11 @@ int main(int argc, char * argv[])
 			exitLevel = ULogger::kError;
 			continue;
 		}
+		if(strcmp(argv[i], "-log_console") == 0)
+		{
+			logConsole = true;
+			continue;
+		}
 
 		// Check for RTAB-Map's parameters
 		std::string key = argv[i];
@@ -289,7 +296,10 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
-	ULogger::setType(ULogger::kTypeConsole);
+	if(logConsole)
+	{
+		ULogger::setType(ULogger::kTypeConsole);
+	}
 	//ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false);
 	//ULogger::setBuffered(true);
 	ULogger::setLevel(logLevel);
@@ -302,11 +312,11 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, startAt, false, false, false, 1.0f/rate);
+		camera = new CameraImages(path, startAt, false, false, false, rate>0.0f?1.0f/rate:0.0f);
 	}
 	else
 	{
-		camera = new CameraVideo(path, false, 1.0f/rate);
+		camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f);
 	}
 
 	if(!camera || !camera->init())