Skip to content

Cplusplus Loop Closure Detection

Jump to bottom
matlabbe edited this page Feb 21, 2023 · 12 revisions

Introduction

This tutorial will show an usage example of the RTAB-Map library in C++.

Details

Project configuration (example with CMake)

Here our project directory:

MyProject
|
-->CMakeLists.txt
-->main.cpp

Files:

In your 'CMakeLists.txt':

cmake_minimum_required(VERSION 2.8)
PROJECT( MyProject )

FIND_PACKAGE(RTABMap REQUIRED)

ADD_EXECUTABLE(example main.cpp)
TARGET_LINK_LIBRARIES(example rtabmap)

Note that OpenCV and RTABMap dependencies should be found automatically by cmake if they are installed correctly (CMake looks for installed OpenCVConfig.cmake and RTABMapConfig.cmake).

Code

The 'main.cpp':

#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/CameraRGB.h"
#include <opencv2/core/core.hpp>
#include "rtabmap/utilite/UFile.h"
#include <stdio.h>

void showUsage()
{
   printf("\nUsage:\n"
   		"rtabmap-bow_mapping [options] \"path\"\n"
   		"  path       Path to a directory of images\n "
   		"  Options:"
   		"     -l   localization mode: use already built RTAB-Map database to localize\n ");
   exit(1);
}

int main(int argc, char * argv[])
{
   //ULogger::setType(ULogger::kTypeConsole);
   //ULogger::setLevel(ULogger::kDebug);

   std::string path;
   bool localizationMode = false;

   if(argc < 2)
   {
   	showUsage();
   }

   for(int i=1; i<argc-1; ++i)
   {
   	if(strcmp(argv[i], "-l") == 0)
   	{
   		localizationMode = true;
   	}
   	else
   	{
   		printf("Unrecognized option \"%s\"\n", argv[i]);
   		showUsage();
   	}
   }

   path = argv[argc-1];

   // rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
   rtabmap::CameraImages camera(path);
   if(!camera.init())
   {
   	printf("Camera init failed, using path \"%s\"\n", path.c_str());
   	exit(1);
   }

   // Create RTAB-Map
   rtabmap::Rtabmap rtabmap;

   // Set the time threshold
   rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit

   // To set other parameters, the Parameters interface must be used (Parameters.h).
   // Example here to change the loop closure threshold (default 0.15).
   // Lower the threshold, more loop closures are detected but there is more chance of false positives.
   rtabmap::ParametersMap parameters;
   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));

   // The time threshold set above is also a parameter, one could have set it the same way:
   //   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "700"));
   // Or SURF hessian treshold:
   //   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));

   // Appearance-based only, disable RGB-D mode
   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));

   std::string databasePath = rtabmap::Parameters::defaultRtabmapWorkingDirectory()+"/"+rtabmap::Parameters::getDefaultDatabaseName();
   if(localizationMode)
   {
   	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
   	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
   	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
   }
   else
   {
   	// delete previous database if there's one...
   	UFile::erase(databasePath);
   }

   // Initialize rtabmap: delete/create database...
   rtabmap.init(parameters, databasePath);

   // Process each image of the directory...
   printf("\nProcessing images... from directory \"%s\"\n", path.c_str());

   int countLoopDetected=0;
   int i=0;
   rtabmap::SensorData data = camera.takeImage();
   int nextIndex = rtabmap.getLastLocationId()+1;
   while(!data.imageRaw().empty())
   {
   	// Process image : Main loop of RTAB-Map
   	rtabmap.process(data.imageRaw(), nextIndex);

   	// Check if a loop closure is detected and print some info
   	if(rtabmap.getLoopClosureId())
   	{
   		++countLoopDetected;
   	}
   	++i;
   	if(rtabmap.getLoopClosureId())
   	{
   		printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
   				i,
   				rtabmap.getLastProcessTime(),
   				(int)rtabmap.getSTM().size(), // short-term memory
   				(int)rtabmap.getWM().size(), // working memory
   				rtabmap.getLoopClosureId(),
   				rtabmap.getLoopClosureValue(),
   				nextIndex,
   				rtabmap.getLoopClosureId());
   	}
   	else
   	{
   		printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
   				i,
   				rtabmap.getLastProcessTime(),
   				(int)rtabmap.getSTM().size(), // short-term memory
   				(int)rtabmap.getWM().size(), // working memory
   				rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
   				rtabmap.getLoopClosureValue());
   	}

   	++nextIndex;

   	//Get next image
   	data = camera.takeImage();
   }

   printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);

   // Generate a graph for visualization with Graphiz
   rtabmap.generateDOTGraph("Graph.dot");
   printf("Generated graph \"Graph.dot\", viewable with Graphiz using \"neato -Tpdf Graph.dot -o out.pdf\"\n");

   // Cleanup... save database and logs
   printf("Saving Long-Term Memory to \"rtabmap.db\"...\n");
   rtabmap.close();

   return 0;
}

Note that an exhaustive list of parameters can be found in Parameters.h.

Build and run

In your directory:

$ cmake .
$ make
$ ./example "PATH_TO_A_DIRECTORY_OF_IMAGES"

For image samples to test, download this... so you can do:

./example samples

Output

You should see something like this:

$ ./example samples

Processing images... from directory "samples"
#1 ptime(0.107500s) STM(1) WM(0) hyp(0) value(0.00)
#2 ptime(0.104422s) STM(1) WM(0) hyp(0) value(0.00)
#3 ptime(0.109514s) STM(1) WM(0) hyp(0) value(0.00)
#4 ptime(0.117873s) STM(1) WM(0) hyp(0) value(0.00)
#5 ptime(0.094277s) STM(1) WM(0) hyp(0) value(0.00)
#6 ptime(0.127060s) STM(2) WM(0) hyp(0) value(0.00)
#7 ptime(0.111087s) STM(3) WM(0) hyp(0) value(0.00)
#8 ptime(0.085847s) STM(3) WM(0) hyp(0) value(0.00)
#9 ptime(0.090047s) STM(4) WM(0) hyp(0) value(0.00)
#10 ptime(0.098240s) STM(5) WM(0) hyp(0) value(0.00)
#11 ptime(0.079423s) STM(5) WM(0) hyp(0) value(0.00)
#12 ptime(0.089312s) STM(6) WM(0) hyp(0) value(0.00)
#13 ptime(0.119825s) STM(6) WM(0) hyp(0) value(0.00)
#14 ptime(0.115073s) STM(7) WM(0) hyp(0) value(0.00)
#15 ptime(0.102635s) STM(7) WM(0) hyp(0) value(0.00)
#16 ptime(0.067021s) STM(8) WM(0) hyp(0) value(0.00)
#17 ptime(0.064624s) STM(9) WM(0) hyp(0) value(0.00)
#18 ptime(0.081078s) STM(10) WM(0) hyp(0) value(0.00)
#19 ptime(0.041414s) STM(10) WM(0) hyp(0) value(0.00)
#20 ptime(0.145719s) STM(11) WM(0) hyp(0) value(0.00)
#21 ptime(0.095820s) STM(12) WM(0) hyp(0) value(0.00)
#22 ptime(0.115345s) STM(13) WM(0) hyp(0) value(0.00)
#23 ptime(0.100772s) STM(14) WM(0) hyp(0) value(0.00)
#24 ptime(0.102717s) STM(15) WM(0) hyp(0) value(0.00)
#25 ptime(0.073335s) STM(16) WM(0) hyp(0) value(0.00)
#26 ptime(0.138999s) STM(17) WM(0) hyp(0) value(0.00)
#27 ptime(0.141726s) STM(17) WM(0) hyp(0) value(0.00)
#28 ptime(0.186972s) STM(18) WM(0) hyp(0) value(0.00)
#29 ptime(0.114186s) STM(18) WM(0) hyp(0) value(0.00)
#30 ptime(0.151340s) STM(19) WM(0) hyp(0) value(0.00)
#31 ptime(0.154382s) STM(19) WM(0) hyp(0) value(0.00)
#32 ptime(0.151319s) STM(20) WM(0) hyp(0) value(0.00)
#33 ptime(0.149512s) STM(20) WM(0) hyp(0) value(0.00)
#34 ptime(0.151345s) STM(21) WM(0) hyp(0) value(0.00)
#35 ptime(0.156178s) STM(22) WM(0) hyp(0) value(0.00)
#36 ptime(0.121933s) STM(23) WM(0) hyp(0) value(0.00)
#37 ptime(0.118272s) STM(24) WM(0) hyp(0) value(0.00)
#38 ptime(0.108413s) STM(24) WM(0) hyp(0) value(0.00)
#39 ptime(0.120959s) STM(25) WM(0) hyp(0) value(0.00)
#40 ptime(0.071086s) STM(26) WM(0) hyp(0) value(0.00)
#41 ptime(0.112397s) STM(27) WM(0) hyp(0) value(0.00)
#42 ptime(0.133261s) STM(28) WM(0) hyp(0) value(0.00)
#43 ptime(0.124974s) STM(29) WM(0) hyp(0) value(0.00)
#44 ptime(0.101159s) STM(30) WM(0) hyp(0) value(0.00)
#45 ptime(0.125849s) STM(30) WM(1) hyp(5) value(0.05)
#46 ptime(0.043879s) STM(29) WM(2) hyp(5) value(0.05)
#47 ptime(0.103658s) STM(30) WM(2) hyp(5) value(0.08)
#48 ptime(0.090738s) STM(30) WM(3) hyp(5) value(0.09)
#49 ptime(0.120124s) STM(30) WM(4) hyp(6) value(0.10)
#50 ptime(0.105879s) STM(30) WM(5) hyp(11) value(0.12) *LOOP 50->11*
#51 ptime(0.115391s) STM(30) WM(5) hyp(11) value(0.14) *LOOP 51->11*
#52 ptime(0.122789s) STM(30) WM(6) hyp(13) value(0.14) *LOOP 52->13*
#53 ptime(0.129396s) STM(30) WM(7) hyp(15) value(0.14) *LOOP 53->15*
#54 ptime(0.107797s) STM(30) WM(8) hyp(15) value(0.18) *LOOP 54->15*
#55 ptime(0.106962s) STM(30) WM(9) hyp(16) value(0.10)
#56 ptime(0.068791s) STM(30) WM(10) hyp(17) value(0.15) *LOOP 56->17*
#57 ptime(0.073057s) STM(30) WM(11) hyp(17) value(0.17) *LOOP 57->17*
#58 ptime(0.064110s) STM(30) WM(12) hyp(17) value(0.18) *LOOP 58->17*
#59 ptime(0.052413s) STM(30) WM(13) hyp(17) value(0.17) *LOOP 59->17*
#60 ptime(0.095070s) STM(30) WM(14) hyp(21) value(0.15)
#61 ptime(0.135626s) STM(30) WM(15) hyp(22) value(0.15) *LOOP 61->22*
#62 ptime(0.160219s) STM(30) WM(16) hyp(22) value(0.20) *LOOP 62->22*
#63 ptime(0.113510s) STM(30) WM(17) hyp(23) value(0.21) *LOOP 63->23*
#64 ptime(0.096855s) STM(30) WM(18) hyp(24) value(0.22) *LOOP 64->24*
#65 ptime(0.088582s) STM(30) WM(19) hyp(25) value(0.25) *LOOP 65->25*
#66 ptime(0.129536s) STM(30) WM(20) hyp(27) value(0.28) *LOOP 66->27*
#67 ptime(0.151826s) STM(30) WM(21) hyp(29) value(0.29) *LOOP 67->29*
#68 ptime(0.159780s) STM(30) WM(22) hyp(29) value(0.30) *LOOP 68->29*
#69 ptime(0.176821s) STM(30) WM(23) hyp(31) value(0.29) *LOOP 69->31*
#70 ptime(0.155591s) STM(30) WM(24) hyp(31) value(0.28) *LOOP 70->31*
#71 ptime(0.144815s) STM(30) WM(25) hyp(33) value(0.30) *LOOP 71->33*
#72 ptime(0.171026s) STM(30) WM(26) hyp(33) value(0.31) *LOOP 72->33*
#73 ptime(0.156471s) STM(30) WM(27) hyp(34) value(0.31) *LOOP 73->34*
#74 ptime(0.165909s) STM(30) WM(28) hyp(35) value(0.34) *LOOP 74->35*
#75 ptime(0.137612s) STM(30) WM(29) hyp(36) value(0.37) *LOOP 75->36*
#76 ptime(0.142687s) STM(30) WM(30) hyp(38) value(0.43) *LOOP 76->38*
#77 ptime(0.116558s) STM(30) WM(31) hyp(38) value(0.51) *LOOP 77->38*
#78 ptime(0.114134s) STM(30) WM(32) hyp(39) value(0.62) *LOOP 78->39*
#79 ptime(0.107862s) STM(30) WM(32) hyp(39) value(0.72) *LOOP 79->39*
#80 ptime(0.111557s) STM(30) WM(33) hyp(39) value(0.68) *LOOP 80->39*
#81 ptime(0.138478s) STM(30) WM(34) hyp(41) value(0.62) *LOOP 81->41*
#82 ptime(0.166011s) STM(30) WM(35) hyp(41) value(0.69) *LOOP 82->41*
#83 ptime(0.138795s) STM(30) WM(36) hyp(42) value(0.70) *LOOP 83->42*
#84 ptime(0.123400s) STM(30) WM(37) hyp(43) value(0.68) *LOOP 84->43*
Processing images completed. Loop closures found = 33
Generated graph "Graph.dot", viewable with Graphiz using "neato -Tpdf Graph.dot -o out.pdf"
Saving Long-Term Memory to "LTM.db"...

Print the graph generated using Graphiz:

$ neato -Tpdf Graph.dot -o out.pdf

Clone this wiki locally