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

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