From beab2271f087e289c3844f31249272e8588f0fb5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Sep 2015 12:08:51 -0400 Subject: [PATCH] Added statistics for incremental flann index size --- corelib/include/rtabmap/core/Statistics.h | 2 + corelib/include/rtabmap/core/VWDictionary.h | 2 + corelib/src/Rtabmap.cpp | 10 ++- corelib/src/VWDictionary.cpp | 82 +++++++++++++++++---- tools/ConsoleApp/main.cpp | 16 +++- 5 files changed, 93 insertions(+), 19 deletions(-) diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index ca81b786..e40a49cf 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/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: diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index d70ac4c4..79bb4d85 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/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(); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3631ce55..ee0a2552 100644 --- a/corelib/src/Rtabmap.cpp +++ b/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); diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 7d89e0a3..d1ed18d2 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -83,6 +83,41 @@ public: } } + unsigned int indexedFeatures() const + { + if(!index_) + { + return 0; + } + if(binaryType_) + { + return ((const flann::Index >*)index_)->size(); + + } + else + { + return ((const flann::Index >*)index_)->size(); + } + } + + // return KB + unsigned int memoryUsed() const + { + if(!index_) + { + return 0; + } + if(binaryType_) + { + return ((const flann::Index >*)index_)->usedMemory()/1000; + + } + else + { + return ((const flann::Index >*)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::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + if(_notIndexedWords.size()) { - VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); - 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(_dataTree.rows-1, w->id())); - std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); - if(!inserted.second) + int i = _dataTree.rows; + _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) { - //update to new index - inserted.first->second = _dataTree.rows-1; + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + 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(i, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(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::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::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(_dataTree.rows-1, w->id())); - std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), _dataTree.rows-1)); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); UASSERT(inserted.second); + ++i; } } else diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index 0557c26c..81087809 100644 --- a/tools/ConsoleApp/main.cpp +++ b/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; i0.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())