diff --git a/archive/2010-LoopClosure/README.md b/archive/2010-LoopClosure/README.md new file mode 100644 index 0000000000..b22c0c8265 --- /dev/null +++ b/archive/2010-LoopClosure/README.md @@ -0,0 +1,25 @@ + + To reproduce results (based on parameters of this [paper](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/b/bc/TRO2013.pdf)): + + ``` + rtabmap-console \ + --Rtabmap/StatisticLogged true\ + --Rtabmap/StatisticLoggedHeaders false\ + --Kp/DetectorStrategy 0\ + --Rtabmap/MemoryThr 300\ + --Rtabmap/LoopRatio 0.9\ + --SURF/HessianThreshold 150\ + --Mem/STMSize 30\ + --Vis/MaxFeatures 400\ + --Kp/TfIdfLikelihoodUsed false\ + --Kp/MaxFeatures 400\ + --Kp/BadSignRatio 0.25\ + --Mem/BadSignaturesIgnored true\ + --Mem/RehearsalSimilarity 0.20\ + --Mem/RecentWmRatio 0.2\ + -gt "~/Downloads/UdeS_1Hz.png"\ + ~/Downloads/UdeS_1Hz + ``` +Adding the ground truth file here is optional to show recall at 100% precision at the end of the process directly without using the octave/MATLAB script below. For NewCollege and CityCentre datasets, `rtabmap-imagesJoiner` can be used to assemble the left and right images together. + + To analyze with Octave/MATLAB, drop `LogF.txt` and `LogI.txt` generated files from command above in ShowLogs directly, then execute `showLogs.m`. diff --git a/archive/2010-LoopClosure/README.txt b/archive/2010-LoopClosure/README.txt deleted file mode 100644 index 04258764b2..0000000000 --- a/archive/2010-LoopClosure/README.txt +++ /dev/null @@ -1,4 +0,0 @@ -This directory contains some basic concepts on Bayes filtering. -Main scripts : - RecursivesBayes.m - RecursivesBayesAvpd.m \ No newline at end of file diff --git a/archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m b/archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m index 41304a95ad..1930a61844 100644 --- a/archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m +++ b/archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m @@ -32,8 +32,7 @@ if size(GroundTruth, 1) ~= length(LogF(:,1)) || size(GroundTruth, 1) ~= length(LogI(:,1)) error(['The ground truth size doesn''t match the log files (LogI=' num2str(length(LogI(:,1))) ', LogF=' num2str(length(LogF(:,1))) ', GT=' num2str(size(GroundTruth, 1)) ')']) end - - + %[highestHypot, CorrespondingID, GT, Accepted, Good, Index, UnderLoopRatio] descending order if(sum(LogI(:,8) == 10) > 0) %OLD @@ -111,14 +110,14 @@ index = find(PR(:,1) == 1); if ~isempty(index) maxRecall = PR(index(end),2) * 100; - display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2)))]) + display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2))) '/' num2str(GT_total_positives)]) else display('Recall max (Precision=100%) = 0') end indexAccepted = find(PR(:,3) == 1); if ~isempty(indexAccepted) maxRecall = PR(indexAccepted(end),2) * 100; - display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2)))]) + display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2))) '/' num2str(GT_total_positives)]) else display('Recall max accepted (Precision=100%) = 0') end diff --git a/archive/2010-LoopClosure/ShowLogs/showlogs.m b/archive/2010-LoopClosure/ShowLogs/showlogs.m index c014292a36..553dc7b619 100644 --- a/archive/2010-LoopClosure/ShowLogs/showlogs.m +++ b/archive/2010-LoopClosure/ShowLogs/showlogs.m @@ -22,6 +22,8 @@ set(0,'defaultAxesFontName', 'Times') set(0,'defaultTextFontName', 'Times') +close all + if nargin < 2, GT_file = ''; end if nargin < 1, PathPrefix = '.'; end @@ -325,7 +327,7 @@ x(LogI(:, 1) == 0) = []; plot(x,y, 'g.') -set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))}) +%set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))}) % %matched sign words % y = LogI(:,2); % x = 1:length(y); diff --git a/archive/2010-LoopClosure/run_all.sh b/archive/2010-LoopClosure/run_all.sh new file mode 100755 index 0000000000..ef41d4eca5 --- /dev/null +++ b/archive/2010-LoopClosure/run_all.sh @@ -0,0 +1,8 @@ +#!/bin/bash + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/NewCollege ~/loop_closure_detection_datasets/NewCollege.png +$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/CityCentre ~/loop_closure_detection_datasets/CityCentre.png +$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/UdeS_1Hz ~/loop_closure_detection_datasets/UdeS_1Hz.png + diff --git a/archive/2010-LoopClosure/run_bow.sh b/archive/2010-LoopClosure/run_bow.sh new file mode 100755 index 0000000000..087eceaece --- /dev/null +++ b/archive/2010-LoopClosure/run_bow.sh @@ -0,0 +1,32 @@ +#!/bin/bash + +DATASET_FOLDER="" +GT_FILE="" +if [ $# -eq 2 ] +then + DATASET_FOLDER=$1 + GT_FILE=$2 +else +echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\"" +exit +fi + +rtabmap-console \ + -quiet \ + --Rtabmap/StatisticLogged true\ + --Rtabmap/StatisticLoggedHeaders false\ + --Kp/DetectorStrategy 0\ + --SURF/HessianThreshold 150\ + --Rtabmap/MemoryThr 300\ + --Rtabmap/LoopRatio 0.9\ + --Mem/STMSize 30\ + --Vis/MaxFeatures 400\ + --Kp/TfIdfLikelihoodUsed false\ + --Kp/MaxFeatures 400\ + --Kp/BadSignRatio 0.25\ + --Mem/BadSignaturesIgnored true\ + --Mem/RehearsalSimilarity 0.20\ + --Mem/RecentWmRatio 0.20\ + -gt "$GT_FILE"\ + "$DATASET_FOLDER" + diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index d4b69c1fde..a5acccaf66 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -151,6 +151,8 @@ class RTABMAP_CORE_EXPORT Rtabmap float getTimeThreshold() const {return _maxTimeAllowed;} // in ms void setTimeThreshold(float maxTimeAllowed); // in ms + int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes + void setMemoryThreshold(int maxMemoryAllowed); // in nodes void setInitialPose(const Transform & initialPose); int triggerNewMap(); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index c815fbc8df..b2f9c64102 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2532,37 +2532,37 @@ bool Rtabmap::process( // insert them first to make sure they are loaded. reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end()); } + } - //============================================================ - // RETRIEVAL 3/3 : Load signatures from the database - //============================================================ - if(reactivatedIds.size()) - { - // Not important if the loop closure hypothesis don't have all its neighbors loaded, - // only a loop closure link is added... - signaturesRetrieved = _memory->reactivateSignatures( - reactivatedIds, - _maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved - timeRetrievalDbAccess); + //============================================================ + // RETRIEVAL 3/3 : Load signatures from the database + //============================================================ + if(reactivatedIds.size()) + { + // Not important if the loop closure hypothesis don't have all its neighbors loaded, + // only a loop closure link is added... + signaturesRetrieved = _memory->reactivateSignatures( + reactivatedIds, + _maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved + timeRetrievalDbAccess); - ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess); + ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess); - timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb; - UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess); + timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb; + UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess); - // Immunize just retrieved signatures - immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end()); + // Immunize just retrieved signatures + immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end()); - if(!signaturesRetrieved.empty() && !_globalScanMap.empty()) - { - UWARN("Some signatures have been retrieved from memory management, clearing global scan map..."); - _globalScanMap.clear(); - _globalScanMapPoses.clear(); - } + if(!signaturesRetrieved.empty() && !_globalScanMap.empty()) + { + UWARN("Some signatures have been retrieved from memory management, clearing global scan map..."); + _globalScanMap.clear(); + _globalScanMapPoses.clear(); } - timeReactivations = timer.ticks(); - ULOGGER_INFO("timeReactivations=%fs", timeReactivations); } + timeReactivations = timer.ticks(); + ULOGGER_INFO("timeReactivations=%fs", timeReactivations); //============================================================ // Proximity detections @@ -4739,6 +4739,16 @@ void Rtabmap::setTimeThreshold(float maxTimeAllowed) ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed); } } +void Rtabmap::setMemoryThreshold(int maxMemoryAllowed) +{ + //must be positive, 0 mean inf memory allowed (no memory limit) + _maxMemoryAllowed = maxMemoryAllowed; + if(_maxMemoryAllowed < 0) + { + ULOGGER_WARN("maxMemoryAllowed < 0, then setting it to 0 (inf)."); + _maxMemoryAllowed = 0; + } +} void Rtabmap::setWorkingDirectory(std::string path) { diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index 544d6811a1..22081c5e42 100644 --- a/tools/ConsoleApp/main.cpp +++ b/tools/ConsoleApp/main.cpp @@ -49,17 +49,36 @@ void showUsage() " path For images, use the directory path. For videos or databases, use full\n " " path name\n" "Options:\n" + " -quiet Don't show log for every images.\n" " -rate #.## Acquisition time (seconds)\n" " -rateHz #.## Acquisition rate (Hz), for convenience\n" " -repeat # Repeat the process on the data set # times (minimum of 1)\n" " -createGT Generate a ground truth file\n" + " -gt \"path\" Compute precision/recall with ground truth matrix.\n" " -start_at # When \"path\" is a directory of images, set this parameter\n" " to start processing at image # (default 0).\n" " -skip # Skip X images while reading directory (default 0).\n" " -v Get version of RTAB-Map\n" - " -input \"path\" Load previous database if it exists.\n" - "%s\n", - rtabmap::Parameters::showUsage()); + " -input \"path\" Load previous database if it exists.\n" + "%s\n" + "Example (generating LogI.txt and LogF.txt for rtabmap/archive/2010-LoopClosure/ShowLogs script, and with 2013 paper parameters):\n\n" + " $ rtabmap-console \\\n" + " --Rtabmap/StatisticLogged true\\\n" + " --Rtabmap/StatisticLoggedHeaders false\\\n" + " --Kp/DetectorStrategy 0\\\n" + " --SURF/HessianThreshold 150\\\n" + " --Rtabmap/MemoryThr 300\\\n" + " --Rtabmap/LoopRatio 0.9\\\n" + " --Mem/STMSize 30\\\n" + " --Vis/MaxFeatures 400\\\n" + " --Kp/TfIdfLikelihoodUsed false\\\n" + " --Kp/MaxFeatures 400\\\n" + " --Kp/BadSignRatio 0.25\\\n" + " --Mem/BadSignaturesIgnored true\\\n" + " --Mem/RehearsalSimilarity 0.20\\\n" + " --Mem/RecentWmRatio 0.20\\\n" + " -gt \"~/Downloads/UdeS_1Hz.png\"\\\n" + " ~/Downloads/UdeS_1Hz\n\n", rtabmap::Parameters::showUsage()); exit(1); } @@ -100,8 +119,10 @@ int main(int argc, char * argv[]) int repeat = 0; bool createGT = false; std::string inputDbPath; + std::string gtPath; int startAt = 0; int skip = 0; + bool quiet = false; for(int i=1; i iterationMeanTime; Camera * camera = 0; + int totalImages = 0; if(UDirectory::exists(path)) { camera = new CameraImages(path, rate>0.0f?1.0f/rate:0.0f); @@ -257,7 +297,10 @@ int main(int argc, char * argv[]) exit(1); } - std::map groundTruth; + if(dynamic_cast(camera)) + totalImages = ((CameraImages*)camera)->imagesCount(); + + std::map generatedGroundTruth; // Create tasks Rtabmap rtabmap; @@ -274,8 +317,7 @@ int main(int argc, char * argv[]) printf("Loading database \"%s\".\n", inputDbPath.c_str()); } - // Disable statistics (we don't need them) - uInsert(pm, ParametersPair(Parameters::kRtabmapPublishStats(), "false")); + // Disable RGB-D mode uInsert(pm, ParametersPair(Parameters::kRGBDEnabled(), "false")); // Process an empty image to make sure every libraries are loaded. @@ -287,6 +329,11 @@ int main(int argc, char * argv[]) rtabmap.close(false); ULogger::setLevel(level); + if(quiet) + { + ULogger::setLevel(ULogger::kError); + } + rtabmap.init(pm, inputDbPath); printf("rtabmap init time = %fs\n", timer.ticks()); @@ -295,14 +342,27 @@ int main(int argc, char * argv[]) int loopClosureId; int count = 0; int countLoopDetected=0; - printf("\nParameters : \n"); printf(" Data set : %s\n", path.c_str()); printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold()); + printf(" Memory threshold = %d nodes\n", rtabmap.getMemoryThreshold()); printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate); printf(" Repeating data set = %s\n", repeat?"true":"false"); printf(" Camera starts at image %d (default 0)\n", startAt); printf(" Skip image = %d\n", skip); + cv::Mat inputGT; + if(!gtPath.empty()) + { + if(startAt != 0 || repeat || skip>0) + { + printf(" Cannot input ground truth if startAt,repeat,skip options are used.\n"); + gtPath.clear(); + } + inputGT = cv::imread(gtPath, cv::IMREAD_GRAYSCALE); + printf(" Input ground truth : %s (%dx%d)\n", gtPath.c_str(), inputGT.cols, inputGT.rows); + UASSERT(inputGT.cols == inputGT.rows); + UASSERT(totalImages == 0 || totalImages == inputGT.cols); + } if(createGT) { printf(" Creating the ground truth matrix.\n"); @@ -326,6 +386,7 @@ int main(int argc, char * argv[]) UTimer rtabmapTimer; int imagesProcessed = 0; std::list > teleopActions; + std::map loopClosureStats; while(loopDataset <= repeat && g_forever) { SensorData data = camera->takeImage(); @@ -340,15 +401,32 @@ int main(int argc, char * argv[]) rtabmap.process(data.imageRaw()); double rtabmapTime = rtabmapTimer.elapsed(); loopClosureId = rtabmap.getLoopClosureId(); - if(rtabmap.getLoopClosureId()) + if(loopClosureId) { ++countLoopDetected; } + + if(!gtPath.empty() && rtabmap.getHighestHypothesisValue() > 0.0f) + { + if(i>inputGT.rows || + rtabmap.getHighestHypothesisId()-1 > inputGT.cols) + { + printf("ERROR: Incompatible ground truth file (size=%dx%d, current image index=%d, loop index=%d)!", inputGT.cols, inputGT.rows, i, rtabmap.getHighestHypothesisId()-1); + exit(1); + } + bool rejectedHypothesis = uValue(rtabmap.getStatistics().data(), Statistics::kLoopRejectedHypothesis(), 0.0f) != 0.0f; + unsigned char gtValue = inputGT.at(i, rtabmap.getHighestHypothesisId()-1); + if((gtValue==0 || gtValue == 255) && !rejectedHypothesis) + { + loopClosureStats.insert(std::make_pair(rtabmap.getHighestHypothesisValue(), gtValue==255)); + } + } + for(int j=0; j<=skip; ++j) { data = camera->takeImage(); } - if(++count % 100 == 0) + if(!quiet && ++count % 100 == 0) { printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n", count, countLoopDetected, maxIterationTimeId, maxIterationTime); @@ -372,7 +450,7 @@ int main(int argc, char * argv[]) { if(loopClosureId > 0) { - groundTruth.insert(std::make_pair(i, loopClosureId-1)); + generatedGroundTruth.insert(std::make_pair(i, loopClosureId-1)); } } @@ -388,25 +466,33 @@ int main(int argc, char * argv[]) ULogger::flush(); - if(rtabmap.getLoopClosureId()) - { - printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n", - count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime); - } - else if(rtabmap.getHighestHypothesisId()) - { - printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n", - count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime); - } - else + if(!quiet) { - printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime); - } + if(rtabmap.getLoopClosureId()) + { + printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n", + count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime); + } + else if(rtabmap.getHighestHypothesisId()) + { + printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n", + count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime); + } + else + { + printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime); + } - if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f) + if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f) + { + printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime); + break; // there is problem, don't continue + } + } + else if(totalImages>0 && i % (totalImages/10) == 0) { - printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime); - break; // there is problem, don't continue + printf("."); + fflush(stdout); } } ++loopDataset; @@ -419,18 +505,52 @@ int main(int argc, char * argv[]) printf("Processing images completed. Loop closures found = %d\n", countLoopDetected); printf(" Total time = %fs\n", timer.ticks()); + if(!loopClosureStats.empty()) + { + int totalGoodLoopClosures = 0; + float loopThr = 0.0f; + for(std::map::reverse_iterator iter=loopClosureStats.rbegin(); iter!=loopClosureStats.rend(); ++iter) + { + if(!iter->second) + { + break; + } + loopThr = iter->first; + ++totalGoodLoopClosures; + } + int totalGtLoopClosures = 0; + for(int i=0; i(i,j) == 255) + { + ++totalGtLoopClosures; + break; + } + } + } + + printf(" Recall (100%% Precision): %.2f%% (with %s=%f, accepted=%d/%d)\n", + float(totalGoodLoopClosures)/float(totalGtLoopClosures)*100.0f, + Parameters::kRtabmapLoopThr().c_str(), + loopThr, + totalGoodLoopClosures, + totalGtLoopClosures); + } + if(imagesProcessed && createGT) { - cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U); + cv::Mat generatedGroundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U); - for(std::map::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter) + for(std::map::iterator iter = generatedGroundTruth.begin(); iter!=generatedGroundTruth.end(); ++iter) { - groundTruthMat.at(iter->first, iter->second) = 255; + generatedGroundTruthMat.at(iter->first, iter->second) = 255; } // Generate the ground truth file - printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, groundTruthMat.rows); - cv::imwrite(GENERATED_GT_NAME, groundTruthMat); + printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, generatedGroundTruthMat.rows); + cv::imwrite(GENERATED_GT_NAME, generatedGroundTruthMat); printf(" Creating ground truth file = %fs\n", timer.ticks()); }