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