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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29