tools/Report/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/core/DBDriver.h>
29 #include <rtabmap/core/Graph.h>
30 #include <rtabmap/core/Optimizer.h>
31 #include <rtabmap/utilite/UDirectory.h>
32 #include <rtabmap/utilite/UFile.h>
33 #include <rtabmap/utilite/UStl.h>
34 #include <rtabmap/utilite/UMath.h>
35 #include <rtabmap/utilite/ULogger.h>
36 #include <stdio.h>
37 #include <iostream>
38 #include <fstream>
39 
40 #ifdef WITH_QT
41 #include <rtabmap/utilite/UPlot.h>
42 #include <QApplication>
43 #include <QFile>
44 #endif
45 
46 using namespace rtabmap;
47 
48 void showUsage()
49 {
50  printf("\nUsage:\n"
51  "rtabmap-report [\"Statistic/Id\"] [options] path\n"
52 #ifndef WITH_QT
53  "[Not built with Qt, statistics cannot be plotted]\n"
54 #endif
55  " path Directory containing rtabmap databases or path of a database.\n"
56  " Options:"
57  " --latex Print table formatted in LaTeX with results.\n"
58  " --kitti Compute error based on KITTI benchmark.\n"
59  " --relative Compute relative motion error between poses.\n"
60  " --loop Compute relative motion error of loop closures.\n"
61  " --scale Find the best scale for the map against the ground truth\n"
62  " and compute error based on the scaled path.\n"
63  " --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n"
64  " and valid ground truth indices to [path]_indices.txt \n"
65  " --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n"
66  " override the ground truth set in database if there is one.\n"
67  " If extension is *.db, the optimized poses of that database will\n"
68  " be used as ground truth.\n"
69  " --gt_max_t # Maximum time interval (sec) to interpolate between a pose and\n"
70  " corresponding ground truth (default 1 sec).\n"
71  " --inc Incremental optimization. \n"
72  " --stats Show available statistics \"Statistic/Id\" to plot or get localization statistics (if path is a file). \n"
73 #ifdef WITH_QT
74  " --invert When reading many databases, put all curves from a same \n"
75  " database in same figure, instead of all same curves from \n"
76  " different database in same figure. When reading a single \n"
77  " database, the inverse will be done. \n"
78  " --ids Use IDs for x axis instead of time in the figures. \n"
79  " --start # Start from this node ID for the figures.\n"
80  " --export Export figures' data to txt files.\n"
81  " --export_prefix Prefix to filenames of exported figures' data (default is \"Stat\").\n"
82 #endif
83  " --report Export all evaluation statistics values in report.txt \n"
84  " --loc [#] Show localization statistics for each \"Statistic/Id\" per\n"
85  " session. Optionally set number 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%%\n"
86  " to show cumulative results on console (it is a mask, \n"
87  " we can combine those numbers, e.g., 63 for all) \n"
88  " --loc_delay # Delay to split sessions for localization statistics (default 60 seconds).\n"
89  " --ignore_inter_nodes Ignore intermediate poses and statistics.\n"
90  " --udebug Show debug log.\n"
91  " --help,-h Show usage\n\n");
92  exit(1);
93 }
94 
95 struct LocStats
96 {
97  static LocStats from(const std::vector<float> & array)
98  {
99  LocStats values;
100  values.mean = uMean(array);
101  values.stddev = std::sqrt(uVariance(array, values.mean));
102  uMinMax(array, values.min, values.max);
103  values.total = array.size();
104  values.nonNull = 0.0f;
105  if(!array.empty())
106  {
107  for(size_t j=0; j<array.size(); ++j)
108  {
109  if(array[j] != 0)
110  {
111  values.nonNull+=1.0f;
112  }
113  }
114  values.nonNull = values.nonNull/float(array.size());
115  }
116  return values;
117  }
118  float mean;
119  float stddev;
120  float min;
121  float max;
122  int total;
123  float nonNull;
124 };
125 
126 int main(int argc, char * argv[])
127 {
128  if(argc < 2)
129  {
130  showUsage();
131  }
132 
133  ULogger::setType(ULogger::kTypeConsole);
134  ULogger::setLevel(ULogger::kWarning);
135 #ifdef WITH_QT
136  QApplication app(argc, argv);
137 #endif
138 
139  bool outputLatex = false;
140  bool outputScaled = false;
141  bool outputPoses = false;
142  bool outputKittiError = false;
143  bool outputRelativeError = false;
144  bool outputReport = false;
145  bool outputLoopAccuracy = false;
146  bool incrementalOptimization = false;
147  bool showAvailableStats = false;
148  bool invertFigures = false;
149  bool ignoreInterNodes = false;
150  bool useIds = false;
151  int startId = 0;
152  bool exportFigures = false;
153  std::string exportPrefix = "Stat";
154  int showLoc = 0;
155  float locDelay = 60;
156  std::string gtFile;
157  double gtMaxInterval = 1;
158  std::vector<std::string> statsToShow;
159 #ifdef WITH_QT
160  std::map<std::string, UPlot*> figures;
161 #endif
162  for(int i=1; i<argc; ++i)
163  {
164  if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "--h") == 0)
165  {
166  showUsage();
167  }
168  else if(strcmp(argv[i], "--udebug") == 0)
169  {
170  ULogger::setLevel(ULogger::kDebug);
171  }
172  else if(strcmp(argv[i], "--latex") == 0)
173  {
174  outputLatex = true;
175  }
176  else if(strcmp(argv[i], "--kitti") == 0)
177  {
178  outputKittiError = true;
179  }
180  else if(strcmp(argv[i], "--relative") == 0)
181  {
182  outputRelativeError = true;
183  }
184  else if(strcmp(argv[i], "--scale") == 0)
185  {
186  outputScaled = true;
187  }
188  else if(strcmp(argv[i], "--poses") == 0)
189  {
190  outputPoses = true;
191  }
192  else if(strcmp(argv[i], "--loop") == 0)
193  {
194  outputLoopAccuracy = true;
195  }
196  else if(strcmp(argv[i],"--report") == 0)
197  {
198  outputReport = true;
199  }
200  else if(strcmp(argv[i],"--inc") == 0)
201  {
202  incrementalOptimization = true;
203  }
204  else if(strcmp(argv[i],"--stats") == 0)
205  {
206  showAvailableStats = true;
207  }
208  else if(strcmp(argv[i],"--invert") == 0)
209  {
210  invertFigures = true;
211  }
212  else if(strcmp(argv[i],"--ids") == 0)
213  {
214  useIds = true;
215  }
216  else if(strcmp(argv[i],"--ignore_inter_nodes") == 0)
217  {
218  ignoreInterNodes = true;
219  }
220  else if(strcmp(argv[i],"--export") == 0)
221  {
222  exportFigures = true;
223  }
224  else if(strcmp(argv[i],"--export_prefix") == 0)
225  {
226  ++i;
227  if(i<argc-1)
228  {
229  exportPrefix = argv[i];
230  printf("Export prefix=%s (--export_prefix)\n", exportPrefix.c_str());
231  }
232  else
233  {
234  printf("Missing value for \"--export_prefix\" option.\n");
235  showUsage();
236  }
237  }
238  else if(strcmp(argv[i],"--gt") == 0)
239  {
240  ++i;
241  if(i<argc-1)
242  {
243  gtFile = argv[i];
244  printf("Ground truth file=%s (--gt)\n", gtFile.c_str());
245  if(UFile::getExtension(gtFile) != "db" && UFile::getExtension(gtFile) != "txt")
246  {
247  printf("Wrong file format set for \"--gt\" option.\n");
248  showUsage();
249  }
250  }
251  else
252  {
253  printf("Missing value for \"--gt\" option.\n");
254  showUsage();
255  }
256  }
257  else if(strcmp(argv[i],"--gt_max_t") == 0)
258  {
259  ++i;
260  if(i<argc-1)
261  {
262  gtMaxInterval = uStr2Double(argv[i]);
263  printf("Ground truth max interval=%f sec (--gt_max_t)\n", gtMaxInterval);
264  if(gtMaxInterval<0 || gtMaxInterval>10)
265  {
266  printf("\"--gt_max_t\" option should be between 0 and 10 sec, parsed %f sec.\n", gtMaxInterval);
267  showUsage();
268  }
269  }
270  else
271  {
272  printf("Missing value for \"--gt\" option.\n");
273  showUsage();
274  }
275  }
276  else if(strcmp(argv[i],"--loc") == 0)
277  {
278  ++i;
279  if(i<argc-1)
280  {
281  if(uIsNumber(argv[i]))
282  {
283  showLoc = atoi(argv[i]);
284  printf("Localization statistics=%d (--loc)\n", showLoc);
285  }
286  else
287  {
288  showLoc = -1;
289  --i; // reset
290  printf("Localization statistics (--loc)\n");
291  }
292  }
293  else
294  {
295  printf("Missing type for \"--loc\" option.\n");
296  showUsage();
297  }
298  }
299  else if(strcmp(argv[i],"--loc_delay") == 0)
300  {
301  ++i;
302  if(i<argc-1)
303  {
304  locDelay = atof(argv[i]);
305  printf("Localization delay=%fs (--loc_delay)\n", locDelay);
306  }
307  else
308  {
309  printf("Missing value for \"--loc_delay\" option.\n");
310  showUsage();
311  }
312  }
313  else if(strcmp(argv[i],"--start") == 0)
314  {
315  ++i;
316  if(i<argc-1)
317  {
318  startId = atoi(argv[i]);
319  printf("Figures will be plotted from id=%d (--start)\n", startId);
320  }
321  else
322  {
323  printf("Missing id for \"--start\" option.\n");
324  showUsage();
325  }
326  }
327  else if(i<argc-1)
328  {
329 
330  statsToShow.push_back(argv[i]);
331  }
332  }
333 
334  std::string path = argv[argc-1];
335  path = uReplaceChar(path, '~', UDirectory::homeDir());
336 
337  if(!UDirectory::exists(path) && UFile::getExtension(path).compare("db") == 0)
338  {
339  invertFigures = !invertFigures;
340  }
341  std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > > localizationMultiStats; //<statsName, <Database<Session>> >
342  for(size_t i=0; i<statsToShow.size(); ++i)
343  {
344  std::string figureTitle = statsToShow[i];
345  if(!invertFigures)
346  {
347 #ifdef WITH_QT
348  printf("Plot %s\n", figureTitle.c_str());
349  UPlot * fig = new UPlot();
350  fig->resize(QSize(640,480));
351  fig->setWindowTitle(figureTitle.c_str());
352  if(useIds)
353  {
354  fig->setXLabel("Node ID");
355  }
356  else
357  {
358  fig->setXLabel("Time (s)");
359  }
360  figures.insert(std::make_pair(figureTitle, fig));
361 #endif
362  }
363  if(showLoc & 0b111111)
364  {
365  localizationMultiStats.insert(std::make_pair(figureTitle, std::vector<std::pair<std::string, std::vector<LocStats> > >()));
366  }
367  }
368  if(!invertFigures)
369  {
370  statsToShow.clear();
371  }
372 
373  // External Ground Truth
374  std::map<double, Transform> externalGtPoses;
375  if(!gtFile.empty())
376  {
377  if(UFile::getExtension(gtFile) == "db")
378  {
379  DBDriver * driver = DBDriver::create();
380  if(driver->openConnection(gtFile))
381  {
382  std::map<int, Transform> poses = driver->loadOptimizedPoses();
383  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
384  {
385  Transform p, gt;
386  GPS gps;
387  int m=-1, w=-1;
388  std::string l;
389  double s;
390  std::vector<float> v;
391  EnvSensors sensors;
392  if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors))
393  {
394  externalGtPoses.insert(std::make_pair(s, iter->second));
395  }
396  }
397  }
398  delete driver;
399  }
400  else
401  {
402  // 10 can import format 1, 10 and 11.
403  std::map<int, Transform> poses;
404  std::map<int, double> stamps;
405  if(rtabmap::graph::importPoses(gtFile, 10, poses, 0, &stamps))
406  {
407  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
408  {
409  externalGtPoses.insert(std::make_pair(stamps.at(iter->first), iter->second));
410  }
411  }
412  }
413  if(externalGtPoses.size() < 2)
414  {
415  printf("Failed loading ground truth poses from \"%s\" (\"--gt\" option).\n", gtFile.c_str());
416  showUsage();
417  }
418  else
419  {
420  printf("Loading %ld ground truth poses from \"%s\".\n", externalGtPoses.size(), gtFile.c_str());
421  }
422  }
423 
424  std::string fileName;
425  std::list<std::string> paths;
426  paths.push_back(path);
427  std::vector<std::map<std::string, std::vector<float> > > outputLatexStatistics;
428  std::map<std::string, std::vector<float> > outputLatexStatisticsMap;
429  bool odomRAMSet = false;
430  std::set<std::string> topDirs;
431  while(paths.size())
432  {
433  std::string currentPath = paths.front();
434  UDirectory currentDir(currentPath);
435  paths.pop_front();
436  bool currentPathIsDatabase = false;
437  if(!currentDir.isValid())
438  {
439  if(UFile::getExtension(currentPath).compare("db") == 0)
440  {
441  currentPathIsDatabase=true;
442  printf("Database: %s\n", currentPath.c_str());
443  }
444  else
445  {
446  continue;
447  }
448  }
449  std::list<std::string> subDirs;
450  if(!currentPathIsDatabase)
451  {
452  printf("Directory: %s\n", currentPath.c_str());
453  std::list<std::string> fileNames = currentDir.getFileNames();
454  if(topDirs.empty())
455  {
456  for(std::list<std::string>::iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
457  {
458  topDirs.insert(currentPath+"/"+*iter);
459  }
460  }
461  else
462  {
463  if(topDirs.find(currentPath) != topDirs.end())
464  {
465  if(outputLatexStatisticsMap.size())
466  {
467  outputLatexStatistics.push_back(outputLatexStatisticsMap);
468  outputLatexStatisticsMap.clear();
469  }
470  }
471  }
472  }
473 
474  // For all databases in currentDir
475  while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
476  {
477  int startIdPerDb = startId;
478  if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
479  {
480  std::string filePath;
481  if(currentPathIsDatabase)
482  {
483  filePath = currentPath;
484  fileName = UFile::getName(currentPath);
485  }
486  else
487  {
488  filePath = currentPath + UDirectory::separator() + fileName;
489  }
490 
491  DBDriver * driver = DBDriver::create();
492  ParametersMap params;
493  if(driver->openConnection(filePath))
494  {
495  ULogger::Level logLevel = ULogger::level();
496  if(ULogger::level() == ULogger::kWarning)
497  {
498  ULogger::setLevel(ULogger::kError); // to suppress parameter warnings
499  }
500  params = driver->getLastParameters();
501  ULogger::setLevel(logLevel);
502  std::set<int> ids;
503  driver->getAllNodeIds(ids, false, false, ignoreInterNodes);
504  std::set<int> allIds = ids;
505  if(ignoreInterNodes)
506  {
507  driver->getAllNodeIds(allIds, false, false, false);
508  }
509  std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
510  std::map<int, Transform> odomPoses, gtPoses;
511  std::map<int, double> odomStamps;
512  std::vector<float> cameraTime;
513  cameraTime.reserve(ids.size());
514  std::vector<float> odomTime;
515  odomTime.reserve(ids.size());
516  std::vector<float> slamTime;
517  slamTime.reserve(ids.size());
518  float rmse = -1;
519  float maxRMSE = -1;
520  float maxOdomRAM = -1;
521  float maxMapRAM = -1;
522 #ifdef WITH_QT
523  if(currentPathIsDatabase && showAvailableStats)
524  {
525  std::map<std::string, int> availableStats;
526  for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
527  {
528  if(stats.find(*iter) != stats.end())
529  {
530  for(std::map<std::string, float>::iterator jter=stats.at(*iter).first.begin(); jter!=stats.at(*iter).first.end(); ++jter)
531  {
532  if(availableStats.find(jter->first) != availableStats.end())
533  {
534  ++availableStats.at(jter->first);
535  }
536  else
537  {
538  availableStats.insert(std::make_pair(jter->first, 1));
539  }
540  }
541  }
542  }
543  printf("Showing available statistics in \"%s\":\n", filePath.c_str());
544  for(std::map<std::string, int>::iterator iter=availableStats.begin(); iter!=availableStats.end(); ++iter)
545  {
546  printf("%s (%d)\n", iter->first.c_str(), iter->second);
547  }
548  printf("\n");
549  exit(1);
550  }
551 
552  std::map<std::string, UPlotCurve*> curves;
553  if(statsToShow.empty())
554  {
555  for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
556  {
557  curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
558  if(!localizationMultiStats.empty())
559  localizationMultiStats.at(iter->first).push_back(std::make_pair(fileName, std::vector<LocStats>()));
560  }
561  }
562  else
563  {
564  UPlot * fig = new UPlot();
565  fig->setWindowTitle(filePath.c_str());
566  if(useIds)
567  {
568  fig->setXLabel("Node ID");
569  }
570  else
571  {
572  fig->setXLabel("Time (s)");
573  }
574  if(!figures.insert(std::make_pair(filePath.c_str(), fig)).second)
575  {
576  delete fig;
577  printf("Figure %s already added!\n", filePath.c_str());
578  }
579  else
580  {
581  for(size_t i=0; i<statsToShow.size(); ++i)
582  {
583  curves.insert(std::make_pair(statsToShow[i], fig->addCurve(statsToShow[i].c_str())));
584  if(!localizationMultiStats.empty())
585  localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
586  }
587  }
588  }
589 #else
590  for(size_t i=0; i<statsToShow.size(); ++i)
591  {
592  if(!localizationMultiStats.empty())
593  localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
594  }
595 #endif
596 
597  // Find localization sessions and adjust startId
598  std::set<int> mappingSessionIds;
599  if(!localizationMultiStats.empty())
600  {
601  std::map<int, Transform> poses = driver->loadOptimizedPoses();
602  if(!poses.empty())
603  {
604  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
605  {
606  Transform p, gt;
607  GPS gps;
608  int m=-1, w=-1;
609  std::string l;
610  double s;
611  std::vector<float> v;
612  EnvSensors sensors;
613  if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors))
614  {
615  mappingSessionIds.insert(m);
616  }
617  }
618 
619  if(startIdPerDb ==0)
620  {
621  startIdPerDb = poses.rbegin()->first+1;
622  }
623  }
624  }
625 
626  std::map<std::string, std::vector<float> > localizationSessionStats;
627  double previousStamp = 0.0;
628  std::map<int, int> allWeights;
629  for(std::set<int>::iterator iter=allIds.begin(); iter!=allIds.end(); ++iter)
630  {
631  Transform p, gt;
632  GPS gps;
633  int m=-1, w=-1;
634  std::string l;
635  double s;
636  std::vector<float> v;
637  EnvSensors sensors;
638  if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors))
639  {
640  allWeights.insert(std::make_pair(*iter, w));
641  if((!ignoreInterNodes || w!=-1))
642  {
643  odomPoses.insert(std::make_pair(*iter, p));
644  odomStamps.insert(std::make_pair(*iter, s));
645  if(!externalGtPoses.empty())
646  {
647  std::map<double, rtabmap::Transform>::iterator nextIter = externalGtPoses.upper_bound(s);
648  if(nextIter!=externalGtPoses.end())
649  {
650  std::map<double, rtabmap::Transform>::iterator previousIter = nextIter;
651  --previousIter;
652  if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval))
653  {
654  UASSERT(s-previousIter->first >= 0);
655  gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second)));
656  }
657  }
658  }
659  else if(!gt.isNull())
660  {
661  gtPoses.insert(std::make_pair(*iter, gt));
662  }
663 
664  if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end())
665  {
666  continue;
667  }
668 
669  if(*iter >= startIdPerDb && uContains(stats, *iter))
670  {
671  const std::map<std::string, float> & stat = stats.at(*iter).first;
672  if(uContains(stat, Statistics::kGtTranslational_rmse()))
673  {
674  rmse = stat.at(Statistics::kGtTranslational_rmse());
675  if(maxRMSE==-1 || maxRMSE < rmse)
676  {
677  maxRMSE = rmse;
678  }
679  }
680  if(uContains(stat, std::string("Camera/TotalTime/ms")))
681  {
682  cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
683  }
684  if(uContains(stat, std::string("Odometry/TotalTime/ms")))
685  {
686  odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
687  }
688  else if(uContains(stat, std::string("Odometry/TimeEstimation/ms")))
689  {
690  odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms")));
691  }
692 
693  if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
694  {
695  if(w!=-1)
696  {
697  slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
698  }
699  }
700  else if(uContains(stat, Statistics::kTimingTotal()))
701  {
702  if(w!=-1)
703  {
704  slamTime.push_back(stat.at(Statistics::kTimingTotal()));
705  }
706  }
707 
708  if(uContains(stat, std::string(Statistics::kMemoryRAM_usage())))
709  {
710  float ram = stat.at(Statistics::kMemoryRAM_usage());
711  if(maxMapRAM==-1 || maxMapRAM < ram)
712  {
713  maxMapRAM = ram;
714  }
715  }
716  if(uContains(stat, std::string("Odometry/RAM_usage/MB")))
717  {
718  float ram = stat.at("Odometry/RAM_usage/MB");
719  if(maxOdomRAM==-1 || maxOdomRAM < ram)
720  {
721  maxOdomRAM = ram;
722  }
723  }
724 
725 #ifdef WITH_QT
726  for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
727  {
728 #else
729  for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
730  jter!=localizationMultiStats.end();
731  ++jter)
732  {
733 #endif
734  if(uContains(stat, jter->first))
735  {
736  double y = stat.at(jter->first);
737 #ifdef WITH_QT
738  double x = s;
739  if(useIds)
740  {
741  x = *iter;
742  }
743  jter->second->addValue(x,y);
744 #endif
745 
746  if(!localizationMultiStats.empty())
747  {
748  if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first))
749  {
750  // changed session
751  for(std::map<std::string, std::vector<float> >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter)
752  {
753  LocStats values = LocStats::from(localizationSessionStats.at(kter->first));
754  localizationMultiStats.at(kter->first).rbegin()->second.push_back(values);
755  localizationSessionStats.at(kter->first).clear();
756  }
757 
758  previousStamp = s;
759  }
760 
761  if(!uContains(localizationSessionStats, jter->first))
762  {
763  localizationSessionStats.insert(std::make_pair(jter->first, std::vector<float>()));
764  }
765  localizationSessionStats.at(jter->first).push_back(y);
766  }
767  }
768  }
769  previousStamp = s;
770  }
771  }
772  }
773  }
774 
775  for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
776  jter!=localizationMultiStats.end();
777  ++jter)
778  {
779  if(uContains(localizationSessionStats, jter->first) &&
780  !localizationSessionStats.at(jter->first).empty())
781  {
782  // changed session
783  LocStats values = LocStats::from(localizationSessionStats.at(jter->first));
784  localizationMultiStats.at(jter->first).rbegin()->second.push_back(values);
785  }
786  }
787 
788  std::multimap<int, Link> links;
789  std::multimap<int, Link> allLinks;
790  driver->getAllLinks(allLinks, true, true);
791  if(ignoreInterNodes)
792  {
793  std::multimap<int, Link> allBiLinks;
794  for(std::multimap<int, Link>::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter)
795  {
796  if(jter->second.from() != jter->second.to() &&
797  (jter->second.type() == Link::kNeighbor ||
798  jter->second.type() == Link::kNeighborMerged))
799  {
800  allBiLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
801  }
802  allBiLinks.insert(*jter);
803  }
804  allLinks=allBiLinks;
805  }
806  std::multimap<int, Link> loopClosureLinks;
807  for(std::multimap<int, Link>::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter)
808  {
809  if(jter->second.from() == jter->second.to() || graph::findLink(links, jter->second.from(), jter->second.to(), true) == links.end())
810  {
811  Link link = jter->second;
812  // remove intermediate nodes?
813  if(link.from() != link.to() &&
814  ignoreInterNodes &&
815  (link.type() == Link::kNeighbor ||
816  link.type() == Link::kNeighborMerged))
817  {
818  while(uContains(allWeights, link.to()) && allWeights.at(link.to()) < 0)
819  {
820  std::multimap<int, Link>::iterator uter = allLinks.find(link.to());
821  while(uter != allLinks.end() &&
822  uter->first==link.to() &&
823  uter->second.from()>uter->second.to())
824  {
825  ++uter;
826  }
827  if(uter != allLinks.end())
828  {
829  link = link.merge(uter->second, uter->second.type());
830  allLinks.erase(uter->first);
831  }
832  else
833  {
834  break;
835  }
836  }
837  }
838  links.insert(std::make_pair(jter->first, link));
839  }
840  if( jter->second.type() != Link::kNeighbor &&
841  jter->second.type() != Link::kNeighborMerged &&
842  graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end())
843  {
844  loopClosureLinks.insert(*jter);
845  }
846  }
847 
848  float bestScale = 1.0f;
849  float bestRMSE = -1;
850  float bestRMSEAng = -1;
851  float bestVoRMSE = -1;
852  Transform bestGtToMap = Transform::getIdentity();
853  float kitti_t_err = 0.0f;
854  float kitti_r_err = 0.0f;
855  float relative_t_err = 0.0f;
856  float relative_r_err = 0.0f;
857  float loop_t_err = 0.0f;
858  float loop_r_err = 0.0f;
859 
860  if(ids.size())
861  {
862  std::map<int, Transform> posesOut;
863  std::multimap<int, Link> linksOut;
864  int firstId = *ids.begin();
865  rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
866  bool useOdomGravity = Parameters::defaultMemUseOdomGravity();
867  Parameters::parse(params, Parameters::kMemUseOdomGravity(), useOdomGravity);
868  if(useOdomGravity)
869  {
870  for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
871  {
872  links.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kGravity, iter->second)));
873  }
874  }
875  std::map<int, Transform> posesWithLandmarks = odomPoses;
876  // Added landmark poses if there are some
877  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
878  {
879  if(iter->second.type() == Link::kLandmark)
880  {
881  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
882  if(posesWithLandmarks.find(iter->second.from()) != posesWithLandmarks.end() && posesWithLandmarks.find(iter->second.to()) == posesWithLandmarks.end())
883  {
884  posesWithLandmarks.insert(std::make_pair(iter->second.to(), posesWithLandmarks.at(iter->second.from())*iter->second.transform()));
885  }
886  }
887  }
888  optimizer->getConnectedGraph(firstId, posesWithLandmarks, links, posesOut, linksOut);
889  std::list<std::map<int, Transform> > intermediateGraphes;
890  std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut, incrementalOptimization?&intermediateGraphes:0);
891  if(poses.empty())
892  {
893  // try incremental optimization
894  UWARN("Optimization failed! Try incremental optimization...");
895  poses = optimizer->optimizeIncremental(firstId, posesOut, linksOut);
896  if(poses.empty())
897  {
898  UERROR("Incremental optimization also failed! Only original RMSE will be shown.");
899  bestRMSE = rmse;
900  }
901  else
902  {
903  UWARN("Incremental optimization succeeded!");
904  }
905  }
906 
907  if(poses.size())
908  {
909  //remove landmarks
910  std::map<int, Transform>::iterator iter=poses.begin();
911  while(iter!=poses.end() && iter->first < 0)
912  {
913  poses.erase(iter++);
914  }
915 
916  std::map<int, Transform> groundTruth;
917  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
918  {
919  if(gtPoses.find(iter->first) != gtPoses.end())
920  {
921  groundTruth.insert(*gtPoses.find(iter->first));
922  }
923  }
924 
925  outputScaled = outputScaled && groundTruth.size();
926  for(float scale=outputScaled?0.900f:1.0f; scale<1.100f; scale+=0.001)
927  {
928  std::map<int, Transform> scaledPoses;
929  std::map<int, Transform> scaledOdomPoses;
930 
931  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
932  {
933  Transform t = iter->second.clone();
934  t.x() *= scale;
935  t.y() *= scale;
936  t.z() *= scale;
937  scaledPoses.insert(std::make_pair(iter->first, t));
938 
939  UASSERT(posesOut.find(iter->first)!=posesOut.end());
940  t = posesOut.at(iter->first).clone();
941  t.x() *= scale;
942  t.y() *= scale;
943  t.z() *= scale;
944  scaledOdomPoses.insert(std::make_pair(iter->first, t));
945  }
946  // compute RMSE statistics
947  float translational_rmse = 0.0f;
948  float translational_mean = 0.0f;
949  float translational_median = 0.0f;
950  float translational_std = 0.0f;
951  float translational_min = 0.0f;
952  float translational_max = 0.0f;
953  float rotational_rmse = 0.0f;
954  float rotational_mean = 0.0f;
955  float rotational_median = 0.0f;
956  float rotational_std = 0.0f;
957  float rotational_min = 0.0f;
958  float rotational_max = 0.0f;
959 
960  graph::calcRMSE(
961  groundTruth,
962  scaledOdomPoses,
963  translational_rmse,
964  translational_mean,
965  translational_median,
966  translational_std,
967  translational_min,
968  translational_max,
969  rotational_rmse,
970  rotational_mean,
971  rotational_median,
972  rotational_std,
973  rotational_min,
974  rotational_max);
975  float translational_rmse_vo = translational_rmse;
976 
977  Transform gtToMap = graph::calcRMSE(
978  groundTruth,
979  scaledPoses,
980  translational_rmse,
981  translational_mean,
982  translational_median,
983  translational_std,
984  translational_min,
985  translational_max,
986  rotational_rmse,
987  rotational_mean,
988  rotational_median,
989  rotational_std,
990  rotational_min,
991  rotational_max);
992 
993  if(bestRMSE!=-1 && translational_rmse > bestRMSE)
994  {
995  break;
996  }
997  bestRMSE = translational_rmse;
998  bestVoRMSE = translational_rmse_vo;
999  bestRMSEAng = rotational_rmse;
1000  bestScale = scale;
1001  bestGtToMap = gtToMap;
1002  if(!outputScaled)
1003  {
1004  // just did iteration without any scale, then exit
1005  break;
1006  }
1007  }
1008 
1009  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1010  {
1011  iter->second.x()*=bestScale;
1012  iter->second.y()*=bestScale;
1013  iter->second.z()*=bestScale;
1014  iter->second = bestGtToMap * iter->second;
1015  }
1016 
1017  if(outputRelativeError)
1018  {
1019  if(groundTruth.size() == poses.size())
1020  {
1021  // compute Motion statistics
1022  graph::calcRelativeErrors(uValues(groundTruth), uValues(poses), relative_t_err, relative_r_err);
1023  }
1024  else
1025  {
1026  std::vector<Transform> gtPosesTmp;
1027  std::vector<Transform> rPoses;
1028  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1029  {
1030  if(groundTruth.find(iter->first) != groundTruth.end())
1031  {
1032  gtPosesTmp.push_back(groundTruth.at(iter->first));
1033  rPoses.push_back(poses.at(iter->first));
1034  }
1035  }
1036  if(!gtPosesTmp.empty())
1037  {
1038  graph::calcRelativeErrors(gtPosesTmp, rPoses, relative_t_err, relative_r_err);
1039  }
1040  }
1041  }
1042 
1043  if(outputKittiError)
1044  {
1045  if(groundTruth.size() == poses.size())
1046  {
1047  // compute KITTI statistics
1048  graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), kitti_t_err, kitti_r_err);
1049  }
1050  else
1051  {
1052  printf("Cannot compute KITTI statistics as optimized poses and ground truth don't have the same size (%d vs %d).\n",
1053  (int)poses.size(), (int)groundTruth.size());
1054  }
1055  }
1056 
1057  if(outputPoses)
1058  {
1059  std::string dir = UDirectory::getDir(filePath);
1060  std::string dbName = UFile::getName(filePath);
1061  dbName = dbName.substr(0, dbName.size()-3); // remove db
1062  std::string path = dir+UDirectory::separator()+dbName+"_slam.txt";
1063  std::multimap<int, Link> dummyLinks;
1064  std::map<int, double> stamps;
1065  if(!outputKittiError)
1066  {
1067  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1068  {
1069  UASSERT(odomStamps.find(iter->first) != odomStamps.end());
1070  stamps.insert(*odomStamps.find(iter->first));
1071  }
1072  }
1073  if(!graph::exportPoses(path, outputKittiError?2:10, poses, dummyLinks, stamps))
1074  {
1075  printf("Could not export the poses to \"%s\"!?!\n", path.c_str());
1076  }
1077 
1078  //export odom
1079  path = dir+UDirectory::separator()+dbName+"_odom.txt";
1080  stamps.clear();
1081  if(!outputKittiError)
1082  {
1083  for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
1084  {
1085  UASSERT(odomStamps.find(iter->first) != odomStamps.end());
1086  stamps.insert(*odomStamps.find(iter->first));
1087  }
1088  }
1089  if(!graph::exportPoses(path, outputKittiError?2:10, odomPoses, dummyLinks, stamps))
1090  {
1091  printf("Could not export the odometry to \"%s\"!?!\n", path.c_str());
1092  }
1093 
1094  //export ground truth
1095  if(groundTruth.size())
1096  {
1097  path = dir+UDirectory::separator()+dbName+"_gt.txt";
1098  stamps.clear();
1099  if(!outputKittiError)
1100  {
1101  for(std::map<int, Transform>::iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
1102  {
1103  UASSERT(odomStamps.find(iter->first) != odomStamps.end());
1104  stamps.insert(*odomStamps.find(iter->first));
1105  }
1106  }
1107  if(!graph::exportPoses(path, outputKittiError?2:10, groundTruth, dummyLinks, stamps))
1108  {
1109  printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
1110  }
1111  }
1112  }
1113 
1114  if (outputReport)
1115  {
1116  bool fillHeader = false;
1117  std::ifstream f("report.csv");
1118  if(!f.good())
1119  {
1120  fillHeader = true;
1121  }
1122 
1123  std::ofstream myfile;
1124  myfile.open("report.csv", std::fstream::in | std::fstream::out | std::fstream::app);
1125  if(fillHeader)
1126  {
1127  myfile << "Rosbag name"<<";"<<"error linear (m)"<<";"<<"error linear max (m)"<<";"<<"error linear odom (m)"<<";"
1128  <<"error angular"<<";"
1129  <<"Slam avg (hz)"<<";"<<"Slam max (hz)"<<";"
1130  <<"Odom avg (hz)"<<";"<<"Odom max (hz)"<<std::endl;
1131  }
1132 
1133  myfile <<fileName.c_str()<<";"
1134  <<bestRMSE<< ";"
1135  <<maxRMSE<<";"
1136  <<bestVoRMSE<<";"
1137  <<bestRMSEAng<<";"
1138  <<(1/(uMean(slamTime)/1000.0))<<";"
1139  <<(1/(uMax(slamTime)/1000.0))<<";"
1140  <<(1/(uMean(odomTime)/1000.0))<<";"
1141  <<(1/(uMax(odomTime)/1000.0))<<";"<<std::endl;
1142  myfile.close();
1143  }
1144 
1145  if(outputLoopAccuracy && !groundTruth.empty() && !linksOut.empty())
1146  {
1147  float sumDist = 0.0f;
1148  float sumAngle = 0.0f;
1149  int count = 0;
1150  for(std::multimap<int, Link>::iterator iter=loopClosureLinks.begin(); iter!=loopClosureLinks.end(); ++iter)
1151  {
1152  if( groundTruth.find(iter->second.from())!=groundTruth.end() &&
1153  groundTruth.find(iter->second.to())!=groundTruth.end())
1154  {
1155  Transform gtLink = groundTruth.at(iter->second.from()).inverse()*groundTruth.at(iter->second.to());
1156  const Transform & t = iter->second.transform();
1157  Transform scaledLink(
1158  t.r11(), t.r12(), t.r13(), t.x()*bestScale,
1159  t.r21(), t.r22(), t.r23(), t.y()*bestScale,
1160  t.r31(), t.r32(), t.r33(), t.z()*bestScale);
1161  Transform diff = gtLink.inverse()*scaledLink;
1162  sumDist += diff.getNorm();
1163  sumAngle += diff.getAngle();
1164  ++count;
1165  }
1166  }
1167  if(count>0)
1168  {
1169  loop_t_err = sumDist/float(count);
1170  loop_r_err = sumAngle/float(count);
1171  loop_r_err *= 180/CV_PI; // Rotation error (deg)
1172  }
1173  }
1174  }
1175  }
1176  printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%s, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
1177  fileName.c_str(),
1178  (int)ids.size(),
1179  bestScale,
1180  bestRMSE,
1181  maxRMSE!=-1?uFormat("%.3fm", maxRMSE).c_str():"NA",
1182  bestVoRMSE,
1183  bestRMSEAng,
1184  !outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
1185  !outputRelativeError?"":uFormat(", Relative: t_err=%.3fm r_err=%.2f deg", relative_t_err, relative_r_err).c_str(),
1186  !localizationMultiStats.empty()?"loc":"slam",
1187  (int)uMean(slamTime), (int)uMax(slamTime),
1188  (int)loopClosureLinks.size(),
1189  !outputLoopAccuracy?"":uFormat(" (t_err=%.3fm r_err=%.2f deg)", loop_t_err, loop_r_err).c_str(),
1190  (int)uMean(odomTime), (int)uMax(odomTime),
1191  (int)uMean(cameraTime),
1192  maxOdomRAM!=-1.0f?uFormat("RAM odom=%dMB ", (int)maxOdomRAM).c_str():"",
1193  (int)maxMapRAM);
1194 
1195  if(outputLatex)
1196  {
1197  std::vector<float> stats;
1198  stats.push_back(ids.size());
1199  stats.push_back(bestRMSE);
1200  stats.push_back(maxRMSE);
1201  stats.push_back(bestRMSEAng);
1202  stats.push_back(uMean(odomTime));
1203  stats.push_back(uMean(slamTime));
1204  stats.push_back(uMax(slamTime));
1205  stats.push_back(maxOdomRAM);
1206  stats.push_back(maxMapRAM);
1207  outputLatexStatisticsMap.insert(std::make_pair(filePath, stats));
1208 
1209  if(maxOdomRAM != -1.0f)
1210  {
1211  odomRAMSet = true;
1212  }
1213  }
1214  }
1215  driver->closeConnection();
1216  delete driver;
1217  }
1218  else if(uSplit(fileName, '.').size() == 1)
1219  {
1220  //sub directory
1221  subDirs.push_front(currentPath + UDirectory::separator() + fileName);
1222  }
1223  currentPathIsDatabase = false;
1224  }
1225 
1226  if(!localizationMultiStats.empty() && showLoc!=-1)
1227  {
1228  printf("---Localization results---\n");
1229  std::string prefix = "header={";
1230  printf("%s", prefix.c_str());
1231  for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator iter=localizationMultiStats.begin()->second.begin();
1232  iter!=localizationMultiStats.begin()->second.end();)
1233  {
1234  if(iter!=localizationMultiStats.begin()->second.begin())
1235  {
1236  printf("%s", std::string(prefix.size(), ' ').c_str());
1237  }
1238  printf("%s", iter->first.c_str());
1239  ++iter;
1240  if(iter!=localizationMultiStats.begin()->second.end())
1241  {
1242  printf(";\n");
1243  }
1244  }
1245  printf("}\n");
1246 
1247 
1248  for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator iter=localizationMultiStats.begin();
1249  iter!=localizationMultiStats.end();
1250  ++iter)
1251  {
1252  printf("%s\n", iter->first.c_str());
1253  for(int k=0; k<6; ++k)
1254  {
1255  if(showLoc & (0x1 << k))
1256  {
1257  std::string prefix = uFormat(" %s=[",
1258  k==0?"min":
1259  k==1?"max":
1260  k==2?"mean":
1261  k==3?"stddev":
1262  k==4?"total":
1263  "nonnull%");
1264  printf("%s", prefix.c_str());
1265  for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator jter=iter->second.begin(); jter!=iter->second.end();)
1266  {
1267  if(jter!=iter->second.begin())
1268  {
1269  printf("%s", std::string(prefix.size(), ' ').c_str());
1270  }
1271  for(size_t j=0; j<jter->second.size(); ++j)
1272  {
1273  if(k<4)
1274  {
1275  printf("%f",
1276  k==0?jter->second[j].min:
1277  k==1?jter->second[j].max:
1278  k==2?jter->second[j].mean:
1279  jter->second[j].stddev);
1280  }
1281  else if(k==4)
1282  {
1283  printf("%d",jter->second[j].total);
1284  }
1285  else if(k==5)
1286  {
1287  printf("%.2f", (jter->second[j].nonNull*100));
1288  }
1289  if(j+1 < jter->second.size())
1290  {
1291  printf(" ");
1292  }
1293  }
1294  ++jter;
1295  if(jter!=iter->second.end())
1296  {
1297  printf(";\n");
1298  }
1299  }
1300  printf("]\n");
1301  }
1302  }
1303  iter->second.clear();
1304  }
1305  }
1306 
1307  for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
1308  {
1309  paths.push_front(*iter);
1310  }
1311 
1312  if(outputLatexStatisticsMap.size() && paths.empty())
1313  {
1314  outputLatexStatistics.push_back(outputLatexStatisticsMap);
1315  outputLatexStatisticsMap.clear();
1316  }
1317  }
1318 
1319  if(outputLatex && outputLatexStatistics.size())
1320  {
1321  printf("\nLaTeX output:\n----------------\n");
1322  printf("\\begin{table*}[!t]\n");
1323  printf("\\caption{$t_{end}$ is the absolute translational RMSE value at the end "
1324  "of the experiment as $ATE_{max}$ is the maximum during the experiment. "
1325  "$r_{end}$ is rotational RMSE value at the end of the experiment. "
1326  "$o_{avg}$ and $m_{avg}$ are the average computational time "
1327  "for odometry (front-end) and map update (back-end). "
1328  "$m_{avg}$ is the maximum computational time for map update. "
1329  "$O_{end}$ and $M_{end}$ are the RAM usage at the end of the experiment "
1330  "for odometry and map management respectively.}\n");
1331  printf("\\label{}\n");
1332  printf("\\centering\n");
1333  if(odomRAMSet)
1334  {
1335  printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c|c}\n");
1336  printf("\\cline{2-10}\n");
1337  printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $O_{end}$ & $M_{end}$ \\\\\n");
1338  printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) & (MB) \\\\\n");
1339  }
1340  else
1341  {
1342  printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c}\n");
1343  printf("\\cline{2-9}\n");
1344  printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $M_{end}$ \\\\\n");
1345  printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) \\\\\n");
1346  }
1347 
1348  printf("\\hline\n");
1349 
1350  for(unsigned int j=0; j<outputLatexStatistics.size(); ++j)
1351  {
1352  if(outputLatexStatistics[j].size())
1353  {
1354  std::vector<int> lowestIndex;
1355  if(outputLatexStatistics[j].size() > 1)
1356  {
1357  std::vector<float> lowestValue(outputLatexStatistics[j].begin()->second.size(),-1);
1358  lowestIndex = std::vector<int>(lowestValue.size(),0);
1359  int index = 0;
1360  for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
1361  {
1362  UASSERT(lowestValue.size() == iter->second.size());
1363  for(unsigned int i=0; i<iter->second.size(); ++i)
1364  {
1365  if(lowestValue[i] == -1 || (iter->second[i]>0.0f && lowestValue[i]>iter->second[i]))
1366  {
1367  lowestValue[i] = iter->second[i];
1368  lowestIndex[i] = index;
1369  }
1370  }
1371  ++index;
1372  }
1373  }
1374 
1375  int index = 0;
1376  for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
1377  {
1378  UASSERT(iter->second.size() == 9);
1379  printf("%s & ", uReplaceChar(iter->first.c_str(), '_', '-').c_str());
1380  printf("%d & ", (int)iter->second[0]);
1381  printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[1]==index?"\\textbf{":"", iter->second[1], lowestIndex.size()&&lowestIndex[1]==index?"}":"");
1382  printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[2]==index?"\\textbf{":"", iter->second[2], lowestIndex.size()&&lowestIndex[2]==index?"}":"");
1383  printf("%s%.2f%s & ", lowestIndex.size()&&lowestIndex[3]==index?"\\textbf{":"", iter->second[3], lowestIndex.size()&&lowestIndex[3]==index?"}":"");
1384  printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[4]==index?"\\textbf{":"", (int)iter->second[4], lowestIndex.size()&&lowestIndex[4]==index?"}":"");
1385  printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[5]==index?"\\textbf{":"", (int)iter->second[5], lowestIndex.size()&&lowestIndex[5]==index?"}":"");
1386  printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[6]==index?"\\textbf{":"", (int)iter->second[6], lowestIndex.size()&&lowestIndex[6]==index?"}":"");
1387  if(odomRAMSet)
1388  {
1389  printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[7]==index?"\\textbf{":"", (int)iter->second[7], lowestIndex.size()&&lowestIndex[7]==index?"}":"");
1390  }
1391  printf("%s%d%s ", lowestIndex.size()&&lowestIndex[8]==index?"\\textbf{":"", (int)iter->second[8], lowestIndex.size()&&lowestIndex[8]==index?"}":"");
1392  printf("\\\\\n");
1393  ++index;
1394  }
1395  printf("\\hline\n");
1396  }
1397  }
1398 
1399 
1400  printf("\\end{tabular}\n");
1401  printf("\\end{table*}\n----------------\n");
1402  }
1403 #ifdef WITH_QT
1404  if(figures.size())
1405  {
1406  for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
1407  {
1408  if(!useIds)
1409  {
1410  iter->second->frameData();
1411  }
1412  if(exportFigures)
1413  {
1414  QString data = iter->second->getAllCurveDataAsText();
1415  if(!data.isEmpty())
1416  {
1417  QString filePath = QString(exportPrefix.c_str()) + (exportPrefix.empty()?"":"-") + iter->second->windowTitle().replace('/', "-") + ".txt";
1418  QFile file(filePath);
1419  if(file.open(QIODevice::Text | QIODevice::WriteOnly))
1420  {
1421  file.write(data.toUtf8());
1422  file.close();
1423  printf("Exported \"%s\".\n", filePath.toStdString().c_str());
1424  }
1425  else
1426  {
1427  printf("ERROR: could not open file \"%s\" for writing!\n", filePath.toStdString().c_str());
1428  }
1429  }
1430  }
1431  else
1432  {
1433  iter->second->show();
1434  }
1435  }
1436  if(!exportFigures)
1437  {
1438  return app.exec();
1439  }
1440  }
1441 #endif
1442  return 0;
1443 }
w
RowVector3d w
compare
bool compare
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1197
LocStats::from
static LocStats from(const std::vector< float > &array)
Definition: tools/Report/main.cpp:97
uIsNumber
bool uIsNumber(const std::string &str)
Definition: UStl.h:645
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
rtabmap::graph::calcRelativeErrors
void RTABMAP_CORE_EXPORT calcRelativeErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:717
x1
x1
UDirectory::getFileNames
const std::list< std::string > & getFileNames() const
Definition: UDirectory.h:129
ULogger::kError
@ kError
Definition: ULogger.h:252
s
RealScalar s
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
LocStats::nonNull
float nonNull
Definition: tools/Report/main.cpp:123
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::Optimizer
Definition: Optimizer.h:61
count
Index count
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::GPS
Definition: GPS.h:35
ULogger::kDebug
@ kDebug
Definition: ULogger.h:252
UDirectory::separator
static std::string separator()
Definition: UDirectory.cpp:391
y
Matrix3f y
LocStats::min
float min
Definition: tools/Report/main.cpp:120
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
iterator
main
int main(int argc, char *argv[])
Definition: tools/Report/main.cpp:126
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:489
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
LocStats::total
int total
Definition: tools/Report/main.cpp:122
fabs
Real fabs(const Real &a)
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
LocStats::stddev
float stddev
Definition: tools/Report/main.cpp:119
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:649
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
data
int data[]
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
j
std::ptrdiff_t j
stats
bool stats
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
first
constexpr int first(int i)
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
Optimizer.h
UPlot::addCurve
UPlotCurve * addCurve(const QString &curveName, const QColor &color=QColor())
Definition: UPlot.cpp:2134
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors) const
Definition: DBDriver.cpp:786
UASSERT
#define UASSERT(condition)
UPlot::setXLabel
void setXLabel(const QString &text)
Definition: UPlot.cpp:3106
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
x
x
m
Matrix3f m
p
Point3_ p(2)
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::DBDriver::getAllStatistics
std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatistics() const
Definition: DBDriver.cpp:257
DBDriver.h
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:1024
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:399
path
path
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
array::size
ssize_t size() const
rtabmap::DBDriver
Definition: DBDriver.h:62
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
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.
uMinMax
void uMinMax(const T *v, unsigned int size, T &min, T &max, unsigned int &indexMin, unsigned int &indexMax)
Definition: UMath.h:224
UPlot.h
array
rtabmap::Transform
Definition: Transform.h:41
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
Graph.h
UDirectory::getNextFileName
std::string getNextFileName()
Definition: UDirectory.cpp:221
showUsage
void showUsage()
Definition: tools/Report/main.cpp:48
values
leaf::MyValues values
rtabmap::graph::importPoses
bool RTABMAP_CORE_EXPORT importPoses(const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > *constraints=0, std::map< int, double > *stamps=0)
Definition: Graph.cpp:184
iter
iterator iter(handle obj)
rtabmap::Optimizer::optimizeIncremental
std::map< int, Transform > optimizeIncremental(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:334
LocStats::max
float max
Definition: tools/Report/main.cpp:121
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
diff
diff
v
Array< int, Dynamic, 1 > v
scan_step::second
@ second
float
float
ULogger::Level
Level
Definition: ULogger.h:252
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:886
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:92
Vector2::x
float x
LocStats
Definition: tools/Report/main.cpp:95
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
UPlot
Definition: UPlot.h:492
t
Point2 t(10, 10)
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UDirectory
Definition: UDirectory.h:34
LocStats::mean
float mean
Definition: tools/Report/main.cpp:118
UERROR
#define UERROR(...)
file
file
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: DBDriver.cpp:925
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::Optimizer::getConnectedGraph
void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut) const
Definition: Optimizer.cpp:188
UDirectory::isValid
bool isValid()
Definition: UDirectory.cpp:216
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41

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