tools/ConsoleApp/main.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/utilite/ULogger.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include "rtabmap/core/Rtabmap.h"
31 #include "rtabmap/core/CameraRGB.h"
32 #include <rtabmap/utilite/UDirectory.h>
33 #include <rtabmap/utilite/UFile.h>
34 #include <rtabmap/utilite/UConversion.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <fstream>
37 #include <queue>
38 #include <opencv2/core/core.hpp>
39 #include <signal.h>
40 
41 using namespace rtabmap;
42 
43 #define GENERATED_GT_NAME "GroundTruth_generated.bmp"
44 
45 void showUsage()
46 {
47  printf("\nUsage:\n"
48  "rtabmap-console [options] \"path\"\n"
49  " path For images, use the directory path. For videos or databases, use full\n "
50  " path name\n"
51  "Options:\n"
52  " -quiet Don't show log for every images.\n"
53  " -rate #.## Acquisition time (seconds)\n"
54  " -rateHz #.## Acquisition rate (Hz), for convenience\n"
55  " -repeat # Repeat the process on the data set # times (minimum of 1)\n"
56  " -createGT Generate a ground truth file\n"
57  " -gt \"path\" Compute precision/recall with ground truth matrix.\n"
58  " -start_at # When \"path\" is a directory of images, set this parameter\n"
59  " to start processing at image # (default 0).\n"
60  " -skip # Skip X images while reading directory (default 0).\n"
61  " -v Get version of RTAB-Map\n"
62  " -input \"path\" Load previous database if it exists.\n"
63  "%s\n"
64  "Example (generating LogI.txt and LogF.txt for rtabmap/archive/2010-LoopClosure/ShowLogs script, and with 2013 paper parameters):\n\n"
65  " $ rtabmap-console \\\n"
66  " --Rtabmap/StatisticLogged true\\\n"
67  " --Rtabmap/StatisticLoggedHeaders false\\\n"
68  " --Kp/DetectorStrategy 0\\\n"
69  " --SURF/HessianThreshold 150\\\n"
70  " --Rtabmap/MemoryThr 300\\\n"
71  " --Rtabmap/LoopRatio 0.9\\\n"
72  " --Mem/STMSize 30\\\n"
73  " --Vis/MaxFeatures 400\\\n"
74  " --Kp/TfIdfLikelihoodUsed false\\\n"
75  " --Kp/MaxFeatures 400\\\n"
76  " --Kp/BadSignRatio 0.25\\\n"
77  " --Mem/BadSignaturesIgnored true\\\n"
78  " --Mem/RehearsalSimilarity 0.20\\\n"
79  " --Mem/RecentWmRatio 0.20\\\n"
80  " -gt \"~/Downloads/UdeS_1Hz.png\"\\\n"
81  " ~/Downloads/UdeS_1Hz\n\n", rtabmap::Parameters::showUsage());
82  exit(1);
83 }
84 
85 // catch ctrl-c
86 bool g_forever = true;
87 void sighandler(int sig)
88 {
89  printf("\nSignal %d caught...\n", sig);
90  g_forever = false;
91 }
92 
93 int main(int argc, char * argv[])
94 {
95  signal(SIGABRT, &sighandler);
96  signal(SIGTERM, &sighandler);
97  signal(SIGINT, &sighandler);
98 
99  ULogger::setType(ULogger::kTypeConsole);
100  ULogger::setLevel(ULogger::kWarning);
101 
102  ParametersMap pm = Parameters::parseArguments(argc, argv);
103  pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
104 
105  if(argc < 2)
106  {
107  showUsage();
108  }
109  else if(argc == 2 && strcmp(argv[1], "-v") == 0)
110  {
111  printf("%s\n", Parameters::getVersion().c_str());
112  exit(0);
113  }
114  printf("\n");
115 
116  std::string path;
117  float rate = 0.0;
118  int loopDataset = 0;
119  int repeat = 0;
120  bool createGT = false;
121  std::string inputDbPath;
122  std::string gtPath;
123  int startAt = 0;
124  int skip = 0;
125  bool quiet = false;
126 
127  for(int i=1; i<argc; ++i)
128  {
129  if(i == argc-1)
130  {
131  // The last must be the path
132  path = argv[i];
133  if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str()))
134  {
135  printf("Path not valid : %s\n", path.c_str());
136  showUsage();
137  exit(1);
138  }
139  break;
140  }
141  if(strcmp(argv[i], "-rate") == 0)
142  {
143  ++i;
144  if(i < argc)
145  {
146  rate = uStr2Float(argv[i]);
147  if(rate < 0)
148  {
149  showUsage();
150  }
151  }
152  else
153  {
154  showUsage();
155  }
156  continue;
157  }
158  if(strcmp(argv[i], "-rateHz") == 0)
159  {
160  ++i;
161  if(i < argc)
162  {
163  rate = uStr2Float(argv[i]);
164  if(rate < 0)
165  {
166  showUsage();
167  }
168  else if(rate)
169  {
170  rate = 1/rate;
171  }
172  }
173  else
174  {
175  showUsage();
176  }
177  continue;
178  }
179  if(strcmp(argv[i], "-repeat") == 0)
180  {
181  ++i;
182  if(i < argc)
183  {
184  repeat = std::atoi(argv[i]);
185  if(repeat < 1)
186  {
187  showUsage();
188  }
189  }
190  else
191  {
192  showUsage();
193  }
194  continue;
195  }
196  if(strcmp(argv[i], "-start_at") == 0)
197  {
198  ++i;
199  if(i < argc)
200  {
201  startAt = std::atoi(argv[i]);
202  if(startAt < 0)
203  {
204  showUsage();
205  }
206  }
207  else
208  {
209  showUsage();
210  }
211  continue;
212  }
213  if (strcmp(argv[i], "-skip") == 0)
214  {
215  ++i;
216  if (i < argc)
217  {
218  skip = std::atoi(argv[i]);
219  if (skip < 0)
220  {
221  showUsage();
222  }
223  }
224  else
225  {
226  showUsage();
227  }
228  continue;
229  }
230  if(strcmp(argv[i], "-quiet") == 0)
231  {
232  quiet = true;
233  continue;
234  }
235  if(strcmp(argv[i], "-createGT") == 0)
236  {
237  createGT = true;
238  continue;
239  }
240  if(strcmp(argv[i], "-gt") == 0)
241  {
242  ++i;
243  if(i < argc)
244  {
245  gtPath = uReplaceChar(argv[i], '~', UDirectory::homeDir());
246  }
247  else
248  {
249  showUsage();
250  }
251  continue;
252  }
253  if(strcmp(argv[i], "-input") == 0)
254  {
255  ++i;
256  if(i < argc)
257  {
258  inputDbPath = argv[i];
259  }
260  else
261  {
262  showUsage();
263  }
264  continue;
265  }
266  if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
267  {
268  showUsage();
269  }
270  }
271 
272  if(repeat && createGT)
273  {
274  printf("Cannot create a Ground truth if repeat is on.\n");
275  showUsage();
276  }
277 
278  UTimer timer;
279  timer.start();
280  std::queue<double> iterationMeanTime;
281 
282  Camera * camera = 0;
283  int totalImages = 0;
284  if(UDirectory::exists(path))
285  {
286  camera = new CameraImages(path, rate>0.0f?1.0f/rate:0.0f);
287  ((CameraImages*)camera)->setStartIndex(startAt);
288  }
289  else
290  {
291  camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f);
292  }
293 
294  if(!camera || !camera->init())
295  {
296  printf("Camera init failed, using path \"%s\"\n", path.c_str());
297  exit(1);
298  }
299 
300  if(dynamic_cast<CameraImages*>(camera))
301  totalImages = ((CameraImages*)camera)->imagesCount();
302 
303  std::map<int, int> generatedGroundTruth;
304 
305  // Create tasks
306  Rtabmap rtabmap;
307  if(inputDbPath.empty())
308  {
309  inputDbPath = "rtabmapconsole.db";
310  if(UFile::erase(inputDbPath) == 0)
311  {
312  printf("Deleted database \"%s\".\n", inputDbPath.c_str());
313  }
314  }
315  else
316  {
317  printf("Loading database \"%s\".\n", inputDbPath.c_str());
318  }
319 
320  // Disable RGB-D mode
321  uInsert(pm, ParametersPair(Parameters::kRGBDEnabled(), "false"));
322 
323  // Process an empty image to make sure every libraries are loaded.
324  ULogger::Level level = ULogger::level();
325  ULogger::setLevel(ULogger::kError);
326  cv::Mat tmp = cv::Mat::zeros(640,480,CV_8UC1);
327  rtabmap.init(pm);
328  rtabmap.process(tmp);
329  rtabmap.close(false);
330  ULogger::setLevel(level);
331 
332  if(quiet)
333  {
334  ULogger::setLevel(ULogger::kError);
335  }
336 
337  rtabmap.init(pm, inputDbPath);
338 
339  printf("rtabmap init time = %fs\n", timer.ticks());
340 
341  // Start thread's task
342  int loopClosureId;
343  int count = 0;
344  int countLoopDetected=0;
345  printf("\nParameters : \n");
346  printf(" Data set : %s\n", path.c_str());
347  printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
348  printf(" Memory threshold = %d nodes\n", rtabmap.getMemoryThreshold());
349  printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
350  printf(" Repeating data set = %s\n", repeat?"true":"false");
351  printf(" Camera starts at image %d (default 0)\n", startAt);
352  printf(" Skip image = %d\n", skip);
353  cv::Mat inputGT;
354  if(!gtPath.empty())
355  {
356  if(startAt != 0 || repeat || skip>0)
357  {
358  printf(" Cannot input ground truth if startAt,repeat,skip options are used.\n");
359  gtPath.clear();
360  }
361  inputGT = cv::imread(gtPath, cv::IMREAD_GRAYSCALE);
362  printf(" Input ground truth : %s (%dx%d)\n", gtPath.c_str(), inputGT.cols, inputGT.rows);
363  UASSERT(inputGT.cols == inputGT.rows);
364  UASSERT(totalImages == 0 || totalImages == inputGT.cols);
365  }
366  if(createGT)
367  {
368  printf(" Creating the ground truth matrix.\n");
369  }
370  printf(" INFO: All other parameters are set to defaults\n");
371  if(pm.size()>1)
372  {
373  printf(" Overwritten parameters :\n");
374  for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter)
375  {
376  printf(" %s=%s\n",iter->first.c_str(), iter->second.c_str());
377  }
378  }
379  if(rtabmap.getWM().size() || rtabmap.getSTM().size())
380  {
381  printf("[Warning] RTAB-Map database is not empty (%s)\n", inputDbPath.c_str());
382  }
383  printf("\nProcessing images...\n");
384 
385  UTimer iterationTimer;
386  UTimer rtabmapTimer;
387  int imagesProcessed = 0;
388  std::list<std::vector<float> > teleopActions;
389  std::map<float, bool> loopClosureStats;
390  while(loopDataset <= repeat && g_forever)
391  {
392  SensorData data = camera->takeImage();
393  int i=0;
394  double maxIterationTime = 0.0;
395  int maxIterationTimeId = 0;
396  while(!data.imageRaw().empty() && g_forever)
397  {
398  ++imagesProcessed;
399  iterationTimer.start();
400  rtabmapTimer.start();
401  rtabmap.process(data.imageRaw());
402  double rtabmapTime = rtabmapTimer.elapsed();
403  loopClosureId = rtabmap.getLoopClosureId();
404  if(loopClosureId)
405  {
406  ++countLoopDetected;
407  }
408 
409  if(!gtPath.empty() && rtabmap.getHighestHypothesisValue() > 0.0f)
410  {
411  if(i>inputGT.rows ||
412  rtabmap.getHighestHypothesisId()-1 > inputGT.cols)
413  {
414  printf("ERROR: Incompatible ground truth file (size=%dx%d, current image index=%d, loop index=%d)!", inputGT.cols, inputGT.rows, i, rtabmap.getHighestHypothesisId()-1);
415  exit(1);
416  }
417  bool rejectedHypothesis = uValue(rtabmap.getStatistics().data(), Statistics::kLoopRejectedHypothesis(), 0.0f) != 0.0f;
418  unsigned char gtValue = inputGT.at<unsigned char>(i, rtabmap.getHighestHypothesisId()-1);
419  if((gtValue==0 || gtValue == 255) && !rejectedHypothesis)
420  {
421  loopClosureStats.insert(std::make_pair(rtabmap.getHighestHypothesisValue(), gtValue==255));
422  }
423  }
424 
425  for(int j=0; j<=skip; ++j)
426  {
427  data = camera->takeImage();
428  }
429  if(!quiet && ++count % 100 == 0)
430  {
431  printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
432  count, countLoopDetected, maxIterationTimeId, maxIterationTime);
433  maxIterationTime = 0.0;
434  maxIterationTimeId = 0;
435  std::map<int, int> wm = rtabmap.getWeights();
436  printf(" WM(%d)=[", (int)wm.size());
437  for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter)
438  {
439  if(iter != wm.begin())
440  {
441  printf(";");
442  }
443  printf("%d,%d", iter->first, iter->second);
444  }
445  printf("]\n");
446  }
447 
448  // Update generated ground truth matrix
449  if(createGT)
450  {
451  if(loopClosureId > 0)
452  {
453  generatedGroundTruth.insert(std::make_pair(i, loopClosureId-1));
454  }
455  }
456 
457  ++i;
458 
459  double iterationTime = iterationTimer.ticks();
460 
461  if(rtabmapTime > maxIterationTime)
462  {
463  maxIterationTime = rtabmapTime;
464  maxIterationTimeId = count;
465  }
466 
467  ULogger::flush();
468 
469  if(!quiet)
470  {
471  if(rtabmap.getLoopClosureId())
472  {
473  printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
474  count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
475  }
476  else if(rtabmap.getHighestHypothesisId())
477  {
478  printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
479  count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
480  }
481  else
482  {
483  printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
484  }
485 
486  if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f)
487  {
488  printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime);
489  break; // there is problem, don't continue
490  }
491  }
492  else if(totalImages>0 && i % (totalImages/10) == 0)
493  {
494  printf(".");
495  fflush(stdout);
496  }
497  }
498  ++loopDataset;
499  if(loopDataset <= repeat)
500  {
501  camera->init();
502  printf(" Beginning loop %d...\n", loopDataset);
503  }
504  }
505  printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
506  printf(" Total time = %fs\n", timer.ticks());
507 
508  if(!loopClosureStats.empty())
509  {
510  int totalGoodLoopClosures = 0;
511  float loopThr = 0.0f;
512  for(std::map<float, bool>::reverse_iterator iter=loopClosureStats.rbegin(); iter!=loopClosureStats.rend(); ++iter)
513  {
514  if(!iter->second)
515  {
516  break;
517  }
518  loopThr = iter->first;
519  ++totalGoodLoopClosures;
520  }
521  int totalGtLoopClosures = 0;
522  for(int i=0; i<inputGT.rows; ++i)
523  {
524  for(int j=0; j<inputGT.cols; ++j)
525  {
526  if(inputGT.at<unsigned char>(i,j) == 255)
527  {
528  ++totalGtLoopClosures;
529  break;
530  }
531  }
532  }
533 
534  printf(" Recall (100%% Precision): %.2f%% (with %s=%f, accepted=%d/%d)\n",
535  float(totalGoodLoopClosures)/float(totalGtLoopClosures)*100.0f,
536  Parameters::kRtabmapLoopThr().c_str(),
537  loopThr,
538  totalGoodLoopClosures,
539  totalGtLoopClosures);
540  }
541 
542  if(imagesProcessed && createGT)
543  {
544  cv::Mat generatedGroundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
545 
546  for(std::map<int, int>::iterator iter = generatedGroundTruth.begin(); iter!=generatedGroundTruth.end(); ++iter)
547  {
548  generatedGroundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
549  }
550 
551  // Generate the ground truth file
552  printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, generatedGroundTruthMat.rows);
553  cv::imwrite(GENERATED_GT_NAME, generatedGroundTruthMat);
554  printf(" Creating ground truth file = %fs\n", timer.ticks());
555  }
556 
557  if(camera)
558  {
559  delete camera;
560  camera = 0 ;
561  }
562 
563  rtabmap.close();
564 
565  printf(" Cleanup time = %fs\n", timer.ticks());
566 
567  printf("Database (\"%s\") and log files saved to current directory.\n", inputDbPath.c_str());
568 
569  return 0;
570 }
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::CameraImages
Definition: CameraImages.h:39
sighandler
void sighandler(int sig)
Definition: tools/ConsoleApp/main.cpp:87
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
ULogger::kError
@ kError
Definition: ULogger.h:252
timer
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:599
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
count
Index count
UTimer::start
void start()
Definition: UTimer.cpp:87
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
stdout
stdout
UTimer.h
rtabmap::Parameters::getVersion
static std::string getVersion()
Definition: Parameters.cpp:82
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::SensorCapture::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
data
int data[]
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
Rtabmap.h
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
repeat
constexpr array< t, n > repeat(t v)
UASSERT
#define UASSERT(condition)
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
level
level
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
CameraRGB.h
rtabmap::Camera
Definition: Camera.h:43
g_forever
bool g_forever
Definition: tools/ConsoleApp/main.cpp:86
path
path
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:572
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uReplaceChar
std::string UTILITE_EXPORT uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
ULogger.h
ULogger class and convenient macros.
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
ULogger::flush
static void flush()
Definition: ULogger.cpp:281
pm
Matrix4d pm
iter
iterator iter(handle obj)
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
UTimer
Definition: UTimer.h:46
showUsage
void showUsage()
Definition: tools/ConsoleApp/main.cpp:45
GENERATED_GT_NAME
#define GENERATED_GT_NAME
Definition: tools/ConsoleApp/main.cpp:43
rtabmap::Rtabmap
Definition: Rtabmap.h:54
ULogger::Level
Level
Definition: ULogger.h:252
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::Camera::takeImage
SensorData takeImage(SensorCaptureInfo *info=0)
Definition: Camera.h:48
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
i
int i
rtabmap::CameraVideo
Definition: CameraVideo.h:36
main
int main(int argc, char *argv[])
Definition: tools/ConsoleApp/main.cpp:93

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12