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 
29 #include <rtabmap/utilite/UTimer.h>
30 #include "rtabmap/core/Rtabmap.h"
31 #include "rtabmap/core/CameraRGB.h"
33 #include <rtabmap/utilite/UFile.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  " -rate #.## Acquisition time (seconds)\n"
53  " -rateHz #.## Acquisition rate (Hz), for convenience\n"
54  " -repeat # Repeat the process on the data set # times (minimum of 1)\n"
55  " -createGT Generate a ground truth file\n"
56  " -start_at # When \"path\" is a directory of images, set this parameter\n"
57  " to start processing at image # (default 0).\n"
58  " -skip # Skip X images while reading directory (default 0).\n"
59  " -v Get version of RTAB-Map\n"
60  " -input \"path\" Load previous database if it exists.\n"
61  "%s\n",
63  exit(1);
64 }
65 
66 // catch ctrl-c
67 bool g_forever = true;
68 void sighandler(int sig)
69 {
70  printf("\nSignal %d caught...\n", sig);
71  g_forever = false;
72 }
73 
74 int main(int argc, char * argv[])
75 {
76  signal(SIGABRT, &sighandler);
77  signal(SIGTERM, &sighandler);
78  signal(SIGINT, &sighandler);
79 
82 
84  pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
85 
86  if(argc < 2)
87  {
88  showUsage();
89  }
90  else if(argc == 2 && strcmp(argv[1], "-v") == 0)
91  {
92  printf("%s\n", Parameters::getVersion().c_str());
93  exit(0);
94  }
95  printf("\n");
96 
97  std::string path;
98  float rate = 0.0;
99  int loopDataset = 0;
100  int repeat = 0;
101  bool createGT = false;
102  std::string inputDbPath;
103  int startAt = 0;
104  int skip = 0;
105 
106  for(int i=1; i<argc; ++i)
107  {
108  if(i == argc-1)
109  {
110  // The last must be the path
111  path = argv[i];
112  if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str()))
113  {
114  printf("Path not valid : %s\n", path.c_str());
115  showUsage();
116  exit(1);
117  }
118  break;
119  }
120  if(strcmp(argv[i], "-rate") == 0)
121  {
122  ++i;
123  if(i < argc)
124  {
125  rate = uStr2Float(argv[i]);
126  if(rate < 0)
127  {
128  showUsage();
129  }
130  }
131  else
132  {
133  showUsage();
134  }
135  continue;
136  }
137  if(strcmp(argv[i], "-rateHz") == 0)
138  {
139  ++i;
140  if(i < argc)
141  {
142  rate = uStr2Float(argv[i]);
143  if(rate < 0)
144  {
145  showUsage();
146  }
147  else if(rate)
148  {
149  rate = 1/rate;
150  }
151  }
152  else
153  {
154  showUsage();
155  }
156  continue;
157  }
158  if(strcmp(argv[i], "-repeat") == 0)
159  {
160  ++i;
161  if(i < argc)
162  {
163  repeat = std::atoi(argv[i]);
164  if(repeat < 1)
165  {
166  showUsage();
167  }
168  }
169  else
170  {
171  showUsage();
172  }
173  continue;
174  }
175  if(strcmp(argv[i], "-start_at") == 0)
176  {
177  ++i;
178  if(i < argc)
179  {
180  startAt = std::atoi(argv[i]);
181  if(startAt < 0)
182  {
183  showUsage();
184  }
185  }
186  else
187  {
188  showUsage();
189  }
190  continue;
191  }
192  if (strcmp(argv[i], "-skip") == 0)
193  {
194  ++i;
195  if (i < argc)
196  {
197  skip = std::atoi(argv[i]);
198  if (skip < 0)
199  {
200  showUsage();
201  }
202  }
203  else
204  {
205  showUsage();
206  }
207  continue;
208  }
209  if(strcmp(argv[i], "-createGT") == 0)
210  {
211  createGT = true;
212  continue;
213  }
214  if(strcmp(argv[i], "-input") == 0)
215  {
216  ++i;
217  if(i < argc)
218  {
219  inputDbPath = argv[i];
220  }
221  else
222  {
223  showUsage();
224  }
225  continue;
226  }
227  if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
228  {
229  showUsage();
230  }
231  }
232 
233  if(repeat && createGT)
234  {
235  printf("Cannot create a Ground truth if repeat is on.\n");
236  showUsage();
237  }
238 
239  UTimer timer;
240  timer.start();
241  std::queue<double> iterationMeanTime;
242 
243  Camera * camera = 0;
244  if(UDirectory::exists(path))
245  {
246  camera = new CameraImages(path, rate>0.0f?1.0f/rate:0.0f);
247  ((CameraImages*)camera)->setStartIndex(startAt);
248  }
249  else
250  {
251  camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f);
252  }
253 
254  if(!camera || !camera->init())
255  {
256  printf("Camera init failed, using path \"%s\"\n", path.c_str());
257  exit(1);
258  }
259 
260  std::map<int, int> groundTruth;
261 
262  // Create tasks
264  if(inputDbPath.empty())
265  {
266  inputDbPath = "rtabmapconsole.db";
267  if(UFile::erase(inputDbPath) == 0)
268  {
269  printf("Deleted database \"%s\".\n", inputDbPath.c_str());
270  }
271  }
272  else
273  {
274  printf("Loading database \"%s\".\n", inputDbPath.c_str());
275  }
276 
277  // Disable statistics (we don't need them)
278  uInsert(pm, ParametersPair(Parameters::kRtabmapPublishStats(), "false"));
279  uInsert(pm, ParametersPair(Parameters::kRGBDEnabled(), "false"));
280 
281  // Process an empty image to make sure every libraries are loaded.
282  ULogger::Level level = ULogger::level();
284  cv::Mat tmp = cv::Mat::zeros(640,480,CV_8UC1);
285  rtabmap.init(pm);
286  rtabmap.process(tmp);
287  rtabmap.close(false);
288  ULogger::setLevel(level);
289 
290  rtabmap.init(pm, inputDbPath);
291 
292  printf("rtabmap init time = %fs\n", timer.ticks());
293 
294  // Start thread's task
295  int loopClosureId;
296  int count = 0;
297  int countLoopDetected=0;
298 
299  printf("\nParameters : \n");
300  printf(" Data set : %s\n", path.c_str());
301  printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
302  printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
303  printf(" Repeating data set = %s\n", repeat?"true":"false");
304  printf(" Camera starts at image %d (default 0)\n", startAt);
305  printf(" Skip image = %d\n", skip);
306  if(createGT)
307  {
308  printf(" Creating the ground truth matrix.\n");
309  }
310  printf(" INFO: All other parameters are set to defaults\n");
311  if(pm.size()>1)
312  {
313  printf(" Overwritten parameters :\n");
314  for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter)
315  {
316  printf(" %s=%s\n",iter->first.c_str(), iter->second.c_str());
317  }
318  }
319  if(rtabmap.getWM().size() || rtabmap.getSTM().size())
320  {
321  printf("[Warning] RTAB-Map database is not empty (%s)\n", inputDbPath.c_str());
322  }
323  printf("\nProcessing images...\n");
324 
325  UTimer iterationTimer;
326  UTimer rtabmapTimer;
327  int imagesProcessed = 0;
328  std::list<std::vector<float> > teleopActions;
329  while(loopDataset <= repeat && g_forever)
330  {
331  SensorData data = camera->takeImage();
332  int i=0;
333  double maxIterationTime = 0.0;
334  int maxIterationTimeId = 0;
335  while(!data.imageRaw().empty() && g_forever)
336  {
337  ++imagesProcessed;
338  iterationTimer.start();
339  rtabmapTimer.start();
340  rtabmap.process(data.imageRaw());
341  double rtabmapTime = rtabmapTimer.elapsed();
342  loopClosureId = rtabmap.getLoopClosureId();
343  if(rtabmap.getLoopClosureId())
344  {
345  ++countLoopDetected;
346  }
347  for(int j=0; j<=skip; ++j)
348  {
349  data = camera->takeImage();
350  }
351  if(++count % 100 == 0)
352  {
353  printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
354  count, countLoopDetected, maxIterationTimeId, maxIterationTime);
355  maxIterationTime = 0.0;
356  maxIterationTimeId = 0;
357  std::map<int, int> wm = rtabmap.getWeights();
358  printf(" WM(%d)=[", (int)wm.size());
359  for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter)
360  {
361  if(iter != wm.begin())
362  {
363  printf(";");
364  }
365  printf("%d,%d", iter->first, iter->second);
366  }
367  printf("]\n");
368  }
369 
370  // Update generated ground truth matrix
371  if(createGT)
372  {
373  if(loopClosureId > 0)
374  {
375  groundTruth.insert(std::make_pair(i, loopClosureId-1));
376  }
377  }
378 
379  ++i;
380 
381  double iterationTime = iterationTimer.ticks();
382 
383  if(rtabmapTime > maxIterationTime)
384  {
385  maxIterationTime = rtabmapTime;
386  maxIterationTimeId = count;
387  }
388 
389  ULogger::flush();
390 
391  if(rtabmap.getLoopClosureId())
392  {
393  printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
394  count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
395  }
396  else if(rtabmap.getHighestHypothesisId())
397  {
398  printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
399  count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
400  }
401  else
402  {
403  printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
404  }
405 
406  if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f)
407  {
408  printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime);
409  break; // there is problem, don't continue
410  }
411  }
412  ++loopDataset;
413  if(loopDataset <= repeat)
414  {
415  camera->init();
416  printf(" Beginning loop %d...\n", loopDataset);
417  }
418  }
419  printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
420  printf(" Total time = %fs\n", timer.ticks());
421 
422  if(imagesProcessed && createGT)
423  {
424  cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
425 
426  for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter)
427  {
428  groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
429  }
430 
431  // Generate the ground truth file
432  printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, groundTruthMat.rows);
433  cv::imwrite(GENERATED_GT_NAME, groundTruthMat);
434  printf(" Creating ground truth file = %fs\n", timer.ticks());
435  }
436 
437  if(camera)
438  {
439  delete camera;
440  camera = 0 ;
441  }
442 
443  rtabmap.close();
444 
445  printf(" Cleanup time = %fs\n", timer.ticks());
446 
447  printf("Database (\"%s\") and log files saved to current directory.\n", inputDbPath.c_str());
448 
449  return 0;
450 }
Definition: UTimer.h:46
#define GENERATED_GT_NAME
std::set< int > getSTM() const
Definition: Rtabmap.cpp:650
std::list< int > getWM() const
Definition: Rtabmap.cpp:619
double elapsed()
Definition: UTimer.h:75
f
static const char * showUsage()
Definition: Parameters.cpp:548
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
static std::string getVersion()
Definition: Parameters.cpp:82
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:575
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
float UTILITE_EXP uStr2Float(const std::string &str)
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:292
void sighandler(int sig)
Some conversion functions.
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
float getLoopClosureValue() const
Definition: Rtabmap.h:128
int getHighestHypothesisId() const
Definition: Rtabmap.h:129
bool g_forever
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:72
Wrappers of STL for convenient functions.
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:392
int getLoopClosureId() const
Definition: Rtabmap.h:127
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
std::map< int, int > getWeights() const
Definition: Rtabmap.cpp:639
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
Definition: Rtabmap.cpp:992
void showUsage()
GLM_FUNC_DECL genType repeat(genType const &Texcoord)
float getTimeThreshold() const
Definition: Rtabmap.h:151
void start()
Definition: UTimer.cpp:87
static ULogger::Level level()
Definition: ULogger.h:340
float getHighestHypothesisValue() const
Definition: Rtabmap.h:130
bool exists()
Definition: UFile.h:104
ULogger class and convenient macros.
double ticks()
Definition: UTimer.cpp:117
int main(int argc, char *argv[])
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
static void flush()
Definition: ULogger.cpp:281
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59