Cplusplus Loop Closure Detection
This tutorial will show an usage example of the RTAB-Map library in C++.
Here our project directory:
MyProject
|
-->CMakeLists.txt
-->main.cpp
Files:
In your 'CMakeLists.txt':
cmake_minimum_required(VERSION 2.8)
PROJECT( MyProject )
SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}")
FIND_PACKAGE(RTABMap REQUIRED)
FIND_PACKAGE(OpenCV REQUIRED)
SET(INCLUDE_DIRS
${RTABMap_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
SET(LIBRARIES
${RTABMap_LIBRARIES}
${OpenCV_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(example main.cpp)
TARGET_LINK_LIBRARIES(example ${LIBRARIES})
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).
The 'main.cpp':
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Camera.h"
#include <opencv2/core/core.hpp>
#include <stdio.h>
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-example \"path\"\n"
" path Path to a directory of images\n ");
exit(1);
}
int main(int argc, char * argv[])
{
if(argc < 2)
{
showUsage();
}
std::string path = argv[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"));
// Initialize rtabmap: load/create database...
rtabmap.init(parameters);
// Process each image of the directory...
printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
int countLoopDetected=0;
int i=0;
cv::Mat img = camera.takeImage();
while(!img.empty())
{
// Process image : Main loop of RTAB-Map
rtabmap.process(img);
// 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(),
rtabmap.getSTM().size(), // short-term memory
rtabmap.getWM().size(), // working memory
rtabmap.getLoopClosureId(),
rtabmap.getLcHypValue(),
rtabmap.getLastLocationId(),
rtabmap.getLoopClosureId());
}
else
{
printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
i,
rtabmap.getLastProcessTime(),
rtabmap.getSTM().size(), // short-term memory
rtabmap.getWM().size(), // working memory
rtabmap.getRetrievedId(), // highest loop closure hypothesis
rtabmap.getLcHypValue());
}
//Get next image
img = camera.takeImage();
}
printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
// Generate a graph for visualization with Graphiz
rtabmap.generateGraph("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 \"LTM.db\"...\n");
rtabmap.close();
return 0;
}
Note that an exhaustive list of parameters can be found in Parameters.h.
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
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
