/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/ULogger.h>
#include <stdio.h>
#include <iostream>
#include <fstream>

#ifdef WITH_QT
#include <rtabmap/utilite/UPlot.h>
#include <QApplication>
#include <QFile>
#endif

using namespace rtabmap;

void showUsage()
{
	printf("\nUsage:\n"
			"rtabmap-report [\"Statistic/Id\"] [options] path\n"
#ifndef WITH_QT
			"[Not built with Qt, statistics cannot be plotted]\n"
#endif
			"  path               Directory containing rtabmap databases or path of a database.\n"
			"  Options:"
			"    --latex            Print table formatted in LaTeX with results.\n"
			"    --kitti            Compute error based on KITTI benchmark.\n"
			"    --relative         Compute relative motion error between poses.\n"
			"    --loop             Compute relative motion error of loop closures.\n"
			"    --scale            Find the best scale for the map against the ground truth\n"
			"                         and compute error based on the scaled path.\n"
			"    --poses            Export odometry to [path]_odom.txt, optimized graph to [path]_slam.txt \n"
			"                         and ground truth to [path]_gt.txt in TUM RGB-D format.\n"
			"    --poses_raw        Same as --poses, but poses are not aligned to gt."
			"    --gt FILE.txt      Use this file as ground truth (TUM RGB-D format). It will\n"
			"                         override the ground truth set in database if there is one.\n"
			"                         If extension is *.db, the optimized poses of that database will\n"
			"                         be used as ground truth.\n"
			"    --gt_max_t #       Maximum time interval (sec) to interpolate between a pose and\n"
			"                         corresponding ground truth (default 1 sec).\n"
			"    --inc              Incremental optimization. \n"
			"    --stats            Show available statistics \"Statistic/Id\" to plot or get localization statistics (if path is a file). \n"
#ifdef WITH_QT
			"    --invert           When reading many databases, put all curves from a same \n"
			"                       database in same figure, instead of all same curves from \n"
			"                       different database in same figure. When reading a single \n"
			"                       database, the inverse will be done. \n"
			"    --ids              Use IDs for x axis instead of time in the figures. \n"
			"    --start #          Start from this node ID for the figures.\n"
			"    --export           Export figures' data to txt files.\n"
			"    --export_prefix    Prefix to filenames of exported figures' data (default is \"Stat\").\n"
#endif
			"    --report           Export all evaluation statistics values in report.txt \n"
			"    --loc [#]          Show localization statistics for each \"Statistic/Id\" per\n"
			"                       session. Optionally set number 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%%\n"
			"                       to show cumulative results on console (it is a mask, \n"
			"                       we can combine those numbers, e.g., 63 for all) \n"
			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds).\n"
			"    --ignore_inter_nodes  Ignore intermediate poses and statistics.\n"
			"    --udebug           Show debug log.\n"
			"    --\"parameter name\" \"value\"  Overwrite a specific RTAB-Map's parameter.\n"
			"    --help,-h          Show usage\n\n");
	exit(1);
}

struct LocStats
{
	static LocStats from(const std::vector<float> & array)
	{
		LocStats values;
		values.mean = uMean(array);
		values.stddev = std::sqrt(uVariance(array, values.mean));
		uMinMax(array, values.min, values.max);
		values.total = array.size();
		values.nonNull = 0.0f;
		if(!array.empty())
		{
			for(size_t j=0; j<array.size(); ++j)
			{
				if(array[j] != 0)
				{
					values.nonNull+=1.0f;
				}
			}
			values.nonNull = values.nonNull/float(array.size());
		}
		return values;
	}
	float mean;
	float stddev;
	float min;
	float max;
	int total;
	float nonNull;
};

int main(int argc, char * argv[])
{
	if(argc < 2)
	{
		showUsage();
	}

	ULogger::setType(ULogger::kTypeConsole);
	ULogger::setLevel(ULogger::kWarning);
#ifdef WITH_QT
	QApplication app(argc, argv);
#endif

	bool outputLatex = false;
	bool outputScaled = false;
	bool outputPoses = false;
	bool outputKittiError = false;
	bool outputRelativeError = false;
	bool outputReport = false;
	bool outputLoopAccuracy = false;
	bool outputPosesAlignedToGt = true;
	bool incrementalOptimization = false;
	bool showAvailableStats = false;
	bool invertFigures = false;
	bool ignoreInterNodes = false;
	bool useIds = false;
	int startId = 0;
	bool exportFigures = false;
	std::string exportPrefix = "Stat";
	int showLoc = 0;
	float locDelay = 60;
	std::string gtFile;
	double gtMaxInterval = 1;
	std::vector<std::string> statsToShow;
#ifdef WITH_QT
	std::map<std::string, UPlot*> figures;
#endif
	ParametersMap overriddenParams = Parameters::parseArguments(argc, argv, true);
	for(int i=1; i<argc; ++i)
	{
		if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "--h") == 0)
		{
			showUsage();
		}
		else if(strcmp(argv[i], "--udebug") == 0)
		{
			ULogger::setLevel(ULogger::kDebug);
		}
		else if(strcmp(argv[i], "--latex") == 0)
		{
			outputLatex = true;
		}
		else if(strcmp(argv[i], "--kitti") == 0)
		{
			outputKittiError = true;
		}
		else if(strcmp(argv[i], "--relative") == 0)
		{
			outputRelativeError = true;
		}
		else if(strcmp(argv[i], "--scale") == 0)
		{
			outputScaled = true;
		}
		else if(strcmp(argv[i], "--poses") == 0)
		{
			outputPoses = true;
		}
		else if(strcmp(argv[i], "--poses_raw") == 0)
		{
			outputPoses = true;
			outputPosesAlignedToGt = false;
		}
		else if(strcmp(argv[i], "--loop") == 0)
		{
			outputLoopAccuracy = true;
		}
		else if(strcmp(argv[i],"--report") == 0)
		{
			outputReport = true;
		}
		else if(strcmp(argv[i],"--inc") == 0)
		{
			incrementalOptimization = true;
		}
		else if(strcmp(argv[i],"--stats") == 0)
		{
			showAvailableStats = true;
		}
		else if(strcmp(argv[i],"--invert") == 0)
		{
			invertFigures = true;
		}
		else if(strcmp(argv[i],"--ids") == 0)
		{
			useIds = true;
		}
		else if(strcmp(argv[i],"--ignore_inter_nodes") == 0)
		{
			ignoreInterNodes = true;
		}
		else if(strcmp(argv[i],"--export") == 0)
		{
			exportFigures = true;
		}
		else if(strcmp(argv[i],"--export_prefix") == 0)
		{
			++i;
			if(i<argc-1)
			{
				exportPrefix = argv[i];
				printf("Export prefix=%s (--export_prefix)\n", exportPrefix.c_str());
			}
			else
			{
				printf("Missing value for \"--export_prefix\" option.\n");
				showUsage();
			}
		}
		else if(strcmp(argv[i],"--gt") == 0)
		{
			++i;
			if(i<argc-1)
			{
				gtFile = argv[i];
				printf("Ground truth file=%s (--gt)\n", gtFile.c_str());
				if(UFile::getExtension(gtFile) != "db" && UFile::getExtension(gtFile) != "txt")
				{
					printf("Wrong file format set for \"--gt\" option.\n");
					showUsage();
				}
			}
			else
			{
				printf("Missing value for \"--gt\" option.\n");
				showUsage();
			}
		}
		else if(strcmp(argv[i],"--gt_max_t") == 0)
		{
			++i;
			if(i<argc-1)
			{
				gtMaxInterval = uStr2Double(argv[i]);
				printf("Ground truth max interval=%f sec (--gt_max_t)\n", gtMaxInterval);
				if(gtMaxInterval<0 || gtMaxInterval>10)
				{
					printf("\"--gt_max_t\" option should be between 0 and 10 sec, parsed %f sec.\n", gtMaxInterval);
					showUsage();
				}
			}
			else
			{
				printf("Missing value for \"--gt\" option.\n");
				showUsage();
			}
		}
		else if(strcmp(argv[i],"--loc") == 0)
		{
			++i;
			if(i<argc-1)
			{
				if(uIsNumber(argv[i]))
				{
					showLoc = atoi(argv[i]);
					printf("Localization statistics=%d (--loc)\n", showLoc);
				}
				else
				{
					showLoc = -1;
					--i; // reset
					printf("Localization statistics (--loc)\n");
				}
			}
			else
			{
				printf("Missing type for \"--loc\" option.\n");
				showUsage();
			}
		}
		else if(strcmp(argv[i],"--loc_delay") == 0)
		{
			++i;
			if(i<argc-1)
			{
				locDelay = atof(argv[i]);
				printf("Localization delay=%fs (--loc_delay)\n", locDelay);
			}
			else
			{
				printf("Missing value for \"--loc_delay\" option.\n");
				showUsage();
			}
		}
		else if(strcmp(argv[i],"--start") == 0)
		{
			++i;
			if(i<argc-1)
			{
				startId = atoi(argv[i]);
				printf("Figures will be plotted from id=%d (--start)\n", startId);
			}
			else
			{
				printf("Missing id for \"--start\" option.\n");
				showUsage();
			}
		}
		else if(uStrContains(argv[i], "--"))
		{
			// Assuming it is a parameter, should be already handled.
			++i;
		}
		else if(i<argc-1)
		{
			statsToShow.push_back(argv[i]);
		}
	}

	if(!overriddenParams.empty())
	{
		printf("Parameters overridden:\n");
		for(auto iter=overriddenParams.begin(); iter!=overriddenParams.end(); ++iter)
		{
			printf("   %s = %s\n", iter->first.c_str(), iter->second.c_str());
		}
	}

	std::string path = argv[argc-1];
	path = uReplaceChar(path, '~', UDirectory::homeDir());

	if(!UDirectory::exists(path) && UFile::getExtension(path).compare("db") == 0)
	{
		invertFigures = !invertFigures;
	}
	std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > > localizationMultiStats; //<statsName, <Database<Session>> >
	for(size_t i=0; i<statsToShow.size(); ++i)
	{
		std::string figureTitle = statsToShow[i];
		if(!invertFigures)
		{
#ifdef WITH_QT
			printf("Plot %s\n", figureTitle.c_str());
			UPlot * fig = new UPlot();
			fig->resize(QSize(640,480));
			fig->setWindowTitle(figureTitle.c_str());
			if(useIds)
			{
				fig->setXLabel("Node ID");
			}
			else
			{
				fig->setXLabel("Time (s)");
			}
			figures.insert(std::make_pair(figureTitle, fig));
#endif
		}
		if(showLoc & 0b111111)
		{
			localizationMultiStats.insert(std::make_pair(figureTitle, std::vector<std::pair<std::string, std::vector<LocStats> > >()));
		}
	}
	if(!invertFigures)
	{
		statsToShow.clear();
	}

	// External Ground Truth
	std::map<double, Transform> externalGtPoses;
	if(!gtFile.empty())
	{
		if(UFile::getExtension(gtFile) == "db")
		{
			DBDriver * driver = DBDriver::create();
			if(driver->openConnection(gtFile))
			{
				std::map<int, Transform> poses = driver->loadOptimizedPoses();
				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
				{
					Transform p, gt;
					GPS gps;
					int m=-1, w=-1;
					std::string l;
					double s;
					std::vector<float> v;
					EnvSensors sensors;
					if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors))
					{
						externalGtPoses.insert(std::make_pair(s, iter->second));
					}
				}
			}
			delete driver;
		}
		else
		{
			// 10 can import format 1, 10 and 11.
			std::map<int, Transform> poses;
			std::map<int, double> stamps;
			if(rtabmap::graph::importPoses(gtFile, 10, poses, 0, &stamps))
			{
				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
				{
					externalGtPoses.insert(std::make_pair(stamps.at(iter->first), iter->second));
				}
			}
		}
		if(externalGtPoses.size() < 2)
		{
			printf("Failed loading ground truth poses from \"%s\" (\"--gt\" option).\n", gtFile.c_str());
			showUsage();
		}
		else
		{
			printf("Loading %ld ground truth poses from \"%s\".\n", externalGtPoses.size(), gtFile.c_str());
		}
	}

	std::string fileName;
	std::list<std::string> paths;
	paths.push_back(path);
	std::vector<std::map<std::string, std::vector<float> > > outputLatexStatistics;
	std::map<std::string, std::vector<float> > outputLatexStatisticsMap;
	bool odomRAMSet = false;
	std::set<std::string> topDirs;
	while(paths.size())
	{
		std::string currentPath = paths.front();
		UDirectory currentDir(currentPath);
		paths.pop_front();
		bool currentPathIsDatabase = false;
		if(!currentDir.isValid())
		{
			if(UFile::getExtension(currentPath).compare("db") == 0)
			{
				currentPathIsDatabase=true;
				printf("Database: %s\n", currentPath.c_str());
			}
			else
			{
				continue;
			}
		}
		std::list<std::string> subDirs;
		if(!currentPathIsDatabase)
		{
			printf("Directory: %s\n", currentPath.c_str());
			std::list<std::string> fileNames = currentDir.getFileNames();
			if(topDirs.empty())
			{
				for(std::list<std::string>::iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
				{
					topDirs.insert(currentPath+"/"+*iter);
				}
			}
			else
			{
				if(topDirs.find(currentPath) != topDirs.end())
				{
					if(outputLatexStatisticsMap.size())
					{
						outputLatexStatistics.push_back(outputLatexStatisticsMap);
						outputLatexStatisticsMap.clear();
					}
				}
			}
		}

		// For all databases in currentDir
		while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
		{
			int startIdPerDb = startId;
			if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
			{
				std::string filePath;
				if(currentPathIsDatabase)
				{
					filePath = currentPath;
					fileName = UFile::getName(currentPath);
				}
				else
				{
					filePath = currentPath + UDirectory::separator() + fileName;
				}

				DBDriver * driver = DBDriver::create();
				ParametersMap params;
				if(driver->openConnection(filePath))
				{
					ULogger::Level logLevel = ULogger::level();
					if(ULogger::level() == ULogger::kWarning)
					{
						ULogger::setLevel(ULogger::kError); // to suppress parameter warnings
					}
					params = driver->getLastParameters();
					uInsert(params, overriddenParams);
					ULogger::setLevel(logLevel);
					std::set<int> ids;
					driver->getAllNodeIds(ids, false, false, ignoreInterNodes);
					std::set<int> allIds = ids;
					if(ignoreInterNodes)
					{
						driver->getAllNodeIds(allIds, false, false, false);
					}
					std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
					std::map<int, Transform> odomPoses, gtPoses;
					std::map<int, double> odomStamps;
					std::vector<float> cameraTime;
					cameraTime.reserve(ids.size());
					std::vector<float> odomTime;
					odomTime.reserve(ids.size());
					std::vector<float> slamTime;
					slamTime.reserve(ids.size());
					float rmse = -1;
					float maxRMSE = -1;
					float maxOdomRAM = -1;
					float maxMapRAM = -1;
#ifdef WITH_QT
					if(currentPathIsDatabase && showAvailableStats)
					{
						std::map<std::string, int> availableStats;
						for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
						{
							if(stats.find(*iter) != stats.end())
							{
								for(std::map<std::string, float>::iterator jter=stats.at(*iter).first.begin(); jter!=stats.at(*iter).first.end(); ++jter)
								{
									if(availableStats.find(jter->first) != availableStats.end())
									{
										++availableStats.at(jter->first);
									}
									else
									{
										availableStats.insert(std::make_pair(jter->first, 1));
									}
								}
							}
						}
						printf("Showing available statistics in \"%s\":\n", filePath.c_str());
						for(std::map<std::string, int>::iterator iter=availableStats.begin(); iter!=availableStats.end(); ++iter)
						{
							printf("%s (%d)\n", iter->first.c_str(), iter->second);
						}
						printf("\n");
						exit(1);
					}

					std::map<std::string, UPlotCurve*> curves;
					if(statsToShow.empty())
					{
						for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
						{
							curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
							if(!localizationMultiStats.empty())
								localizationMultiStats.at(iter->first).push_back(std::make_pair(fileName, std::vector<LocStats>()));
						}
					}
					else
					{
						UPlot * fig = new UPlot();
						fig->setWindowTitle(filePath.c_str());
						if(useIds)
						{
							fig->setXLabel("Node ID");
						}
						else
						{
							fig->setXLabel("Time (s)");
						}
						if(!figures.insert(std::make_pair(filePath.c_str(), fig)).second)
						{
							delete fig;
							printf("Figure %s already added!\n", filePath.c_str());
						}
						else
						{
							for(size_t i=0; i<statsToShow.size(); ++i)
							{
								curves.insert(std::make_pair(statsToShow[i], fig->addCurve(statsToShow[i].c_str())));
								if(!localizationMultiStats.empty())
									localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
							}
						}
					}
#else
					for(size_t i=0; i<statsToShow.size(); ++i)
					{
						if(!localizationMultiStats.empty())
							localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
					}
#endif

					// Find localization sessions and adjust startId
					std::set<int> mappingSessionIds;
					if(!localizationMultiStats.empty())
					{
						std::map<int, Transform> poses = driver->loadOptimizedPoses();
						if(!poses.empty())
						{
							for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
							{
								Transform p, gt;
								GPS gps;
								int m=-1, w=-1;
								std::string l;
								double s;
								std::vector<float> v;
								EnvSensors sensors;
								if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors))
								{
									mappingSessionIds.insert(m);
								}
							}

							if(startIdPerDb ==0)
							{
								startIdPerDb = poses.rbegin()->first+1;
							}
						}
					}

					std::map<std::string, std::vector<float> > localizationSessionStats;
					double previousStamp = 0.0;
					std::map<int, int> allWeights;
					Transform previousPose;
					int previousMapId = 0;
					float totalOdomDistance = 0.0f;
					for(std::set<int>::iterator iter=allIds.begin(); iter!=allIds.end(); ++iter)
					{
						Transform p, gt;
						GPS gps;
						int m=-1, w=-1;
						std::string l;
						double s;
						std::vector<float> v;
						EnvSensors sensors;
						if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors))
						{
							if(previousMapId == m)
							{
								if(!p.isNull() && !previousPose.isNull())
								{
									totalOdomDistance += p.getDistance(previousPose);
								}
							}
							previousPose = p;
							previousMapId = m;

							allWeights.insert(std::make_pair(*iter, w));
							if(!ignoreInterNodes || w!=-1)
							{
								if(w!=-9)
								{
									odomPoses.insert(std::make_pair(*iter, p));
									odomStamps.insert(std::make_pair(*iter, s));
									if(!externalGtPoses.empty())
									{
										std::map<double, rtabmap::Transform>::iterator nextIter = externalGtPoses.upper_bound(s);
										if(nextIter!=externalGtPoses.end())
										{
											std::map<double, rtabmap::Transform>::iterator previousIter = nextIter;
											--previousIter;
											if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval))
											{
												UASSERT(s-previousIter->first >= 0);
												gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second)));
											}
										}
									}
									else if(!gt.isNull())
									{
										gtPoses.insert(std::make_pair(*iter, gt));
									}
								}

								if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end())
								{
									continue;
								}

								if(*iter >= startIdPerDb && uContains(stats, *iter))
								{
									const std::map<std::string, float> & stat = stats.at(*iter).first;
									if(uContains(stat, Statistics::kGtTranslational_rmse()))
									{
										rmse = stat.at(Statistics::kGtTranslational_rmse());
										if(maxRMSE==-1 || maxRMSE < rmse)
										{
											maxRMSE = rmse;
										}
									}
									if(uContains(stat, std::string("Camera/TotalTime/ms")))
									{
										cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
									}
									if(uContains(stat, std::string("Odometry/TotalTime/ms")))
									{
										odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
									}
									else if(uContains(stat, std::string("Odometry/TimeEstimation/ms")))
									{
										odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms")));
									}

									if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
									{
										if(w!=-1)
										{
											slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
										}
									}
									else if(uContains(stat, Statistics::kTimingTotal()))
									{
										if(w!=-1)
										{
											slamTime.push_back(stat.at(Statistics::kTimingTotal()));
										}
									}

									if(uContains(stat, std::string(Statistics::kMemoryRAM_usage())))
									{
										float ram = stat.at(Statistics::kMemoryRAM_usage());
										if(maxMapRAM==-1 || maxMapRAM < ram)
										{
											maxMapRAM = ram;
										}
									}
									if(uContains(stat, std::string("Odometry/RAM_usage/MB")))
									{
										float ram = stat.at("Odometry/RAM_usage/MB");
										if(maxOdomRAM==-1 || maxOdomRAM < ram)
										{
											maxOdomRAM = ram;
										}
									}

#ifdef WITH_QT
									for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
									{
#else
									for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
										jter!=localizationMultiStats.end();
										++jter)
									{
#endif
										if(uContains(stat, jter->first))
										{
											double y = stat.at(jter->first);
#ifdef WITH_QT
											double x = s;
											if(useIds)
											{
												x = *iter;
											}
											jter->second->addValue(x,y);
#endif

											if(!localizationMultiStats.empty())
											{
												if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first))
												{
													// changed session
													for(std::map<std::string, std::vector<float> >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter)
													{
														LocStats values = LocStats::from(localizationSessionStats.at(kter->first));
														localizationMultiStats.at(kter->first).rbegin()->second.push_back(values);
														localizationSessionStats.at(kter->first).clear();
													}

													previousStamp = s;
												}

												if(!uContains(localizationSessionStats, jter->first))
												{
													localizationSessionStats.insert(std::make_pair(jter->first, std::vector<float>()));
												}
												localizationSessionStats.at(jter->first).push_back(y);
											}
										}
									}
									previousStamp = s;
								}
							}
						}
					}

					for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
						jter!=localizationMultiStats.end();
						++jter)
					{
						if(uContains(localizationSessionStats, jter->first) &&
							!localizationSessionStats.at(jter->first).empty())
						{
							// changed session
							LocStats values = LocStats::from(localizationSessionStats.at(jter->first));
							localizationMultiStats.at(jter->first).rbegin()->second.push_back(values);
						}
					}

					std::multimap<int, Link> links;
					std::multimap<int, Link> allLinks;
					driver->getAllLinks(allLinks, true, true);
					if(ignoreInterNodes)
					{
						std::multimap<int, Link> allBiLinks;
						for(std::multimap<int, Link>::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter)
						{
							if(jter->second.from() != jter->second.to() &&
							   (jter->second.type() == Link::kNeighbor ||
							    jter->second.type() == Link::kNeighborMerged))
							{
								allBiLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
							}
							allBiLinks.insert(*jter);
						}
						allLinks=allBiLinks;
					}
					std::multimap<int, Link> loopClosureLinks;
					int landmarks = 0;
					for(std::multimap<int, Link>::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter)
					{
						if(jter->second.from() == jter->second.to() || graph::findLink(links, jter->second.from(), jter->second.to(), true) == links.end())
						{
							Link link = jter->second;
							// remove intermediate nodes?
							if(link.from() != link.to() &&
							   ignoreInterNodes && 
							   (link.type() == Link::kNeighbor ||
							    link.type() == Link::kNeighborMerged))
							{
								while(uContains(allWeights, link.to()) && allWeights.at(link.to()) < 0)
								{
									std::multimap<int, Link>::iterator uter = allLinks.find(link.to());
									while(uter != allLinks.end() && 
										uter->first==link.to() && 
										uter->second.from()>uter->second.to())
									{
										++uter;
									}
									if(uter != allLinks.end())
									{
										link = link.merge(uter->second, uter->second.type());
										allLinks.erase(uter->first);
									}
									else
									{
										break;
									}
								}
							}
							links.insert(std::make_pair(jter->first, link));
						}
						if( jter->second.type() != Link::kNeighbor &&
							jter->second.type() != Link::kNeighborMerged &&
							jter->second.type() != Link::kGravity &&
							jter->second.type() != Link::kLandmark &&
							graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end())
						{
							loopClosureLinks.insert(*jter);
						}
						else if(jter->second.type() == Link::kLandmark)
						{
							landmarks++;
						}
					}

					float bestScale = 1.0f;
					float bestRMSE = -1;
					float bestRMSEAng = -1;
					float bestVoRMSE = -1;
					Transform bestGtToMap = Transform::getIdentity();
					Transform bestGtToOdom = Transform::getIdentity();
					float kitti_t_err = 0.0f;
					float kitti_r_err = 0.0f;
					float relative_t_err = 0.0f;
					float relative_r_err = 0.0f;
					float loop_t_err = 0.0f;
					float loop_r_err = 0.0f;

					if(ids.size())
					{
						std::map<int, Transform> posesOut;
						std::multimap<int, Link> linksOut;
						int firstId = *ids.begin();
						rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
						bool useOdomGravity = Parameters::defaultMemUseOdomGravity();
						Parameters::parse(params, Parameters::kMemUseOdomGravity(), useOdomGravity);
						if(useOdomGravity)
						{
							for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
							{
								links.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kGravity, iter->second)));
							}
						}
						std::map<int, Transform> posesWithLandmarks = odomPoses;
						// Added landmark poses if there are some
						for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
						{
							if(iter->second.type() == Link::kLandmark)
							{
								UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
								if(posesWithLandmarks.find(iter->second.from()) != posesWithLandmarks.end() && posesWithLandmarks.find(iter->second.to()) == posesWithLandmarks.end())
								{
									posesWithLandmarks.insert(std::make_pair(iter->second.to(), posesWithLandmarks.at(iter->second.from())*iter->second.transform()));
								}
							}
						}
						optimizer->getConnectedGraph(firstId, posesWithLandmarks, links, posesOut, linksOut);
						std::list<std::map<int, Transform> > intermediateGraphes;
						std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut, incrementalOptimization?&intermediateGraphes:0);
						if(poses.empty())
						{
							// try incremental optimization
							UWARN("Optimization failed! Try incremental optimization...");
							poses = optimizer->optimizeIncremental(firstId, posesOut, linksOut);
							if(poses.empty())
							{
								UERROR("Incremental optimization also failed! Only original RMSE will be shown.");
								bestRMSE = rmse;
							}
							else
							{
								UWARN("Incremental optimization succeeded!");
							}
						}

						if(poses.size())
						{
							//remove landmarks
							std::map<int, Transform>::iterator iter=poses.begin();
							std::map<int, Transform> optimizedLandmarks;
							while(iter!=poses.end() && iter->first < 0)
							{
								optimizedLandmarks.insert(*iter);
								poses.erase(iter++);
							}
							if(outputKittiError) {
								// remove landmarks
								std::map<int, Transform>::iterator iter=posesOut.begin();
								while(iter!=posesOut.end() && iter->first < 0)
								{
									posesOut.erase(iter++);
								}
							}

							std::map<int, Transform> groundTruth;
							for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
							{
								if(gtPoses.find(iter->first) != gtPoses.end())
								{
									groundTruth.insert(*gtPoses.find(iter->first));
								}
							}

							outputScaled = outputScaled && groundTruth.size();
							for(float scale=outputScaled?0.900f:1.0f; scale<1.100f; scale+=0.001)
							{
								std::map<int, Transform> scaledPoses;
								std::map<int, Transform> scaledOdomPoses;

								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
								{
									Transform t = iter->second.clone();
									t.x() *= scale;
									t.y() *= scale;
									t.z() *= scale;
									scaledPoses.insert(std::make_pair(iter->first, t));

									UASSERT(posesOut.find(iter->first)!=posesOut.end());
									t = posesOut.at(iter->first).clone();
									t.x() *= scale;
									t.y() *= scale;
									t.z() *= scale;
									scaledOdomPoses.insert(std::make_pair(iter->first, t));
								}
								// compute RMSE statistics
								float translational_rmse = 0.0f;
								float translational_mean = 0.0f;
								float translational_median = 0.0f;
								float translational_std = 0.0f;
								float translational_min = 0.0f;
								float translational_max = 0.0f;
								float rotational_rmse = 0.0f;
								float rotational_mean = 0.0f;
								float rotational_median = 0.0f;
								float rotational_std = 0.0f;
								float rotational_min = 0.0f;
								float rotational_max = 0.0f;

								Transform gtToOdom = graph::calcRMSE(
										groundTruth,
										scaledOdomPoses,
										translational_rmse,
										translational_mean,
										translational_median,
										translational_std,
										translational_min,
										translational_max,
										rotational_rmse,
										rotational_mean,
										rotational_median,
										rotational_std,
										rotational_min,
										rotational_max);
								float translational_rmse_vo = translational_rmse;

								Transform gtToMap = graph::calcRMSE(
										groundTruth,
										scaledPoses,
										translational_rmse,
										translational_mean,
										translational_median,
										translational_std,
										translational_min,
										translational_max,
										rotational_rmse,
										rotational_mean,
										rotational_median,
										rotational_std,
										rotational_min,
										rotational_max);

								if(bestRMSE!=-1 && translational_rmse > bestRMSE)
								{
									break;
								}
								bestRMSE = translational_rmse;
								bestVoRMSE = translational_rmse_vo;
								bestRMSEAng = rotational_rmse;
								bestScale = scale;
								bestGtToMap = gtToMap;
								bestGtToOdom = gtToOdom;
								if(!outputScaled)
								{
									// just did iteration without any scale, then exit
									break;
								}
							}

							// Scale/align slam poses
							for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
							{
								iter->second.x()*=bestScale;
								iter->second.y()*=bestScale;
								iter->second.z()*=bestScale;
								if(outputPosesAlignedToGt)
									iter->second = bestGtToMap * iter->second;
							}
							for(std::map<int, Transform>::iterator iter=optimizedLandmarks.begin(); iter!=optimizedLandmarks.end(); ++iter)
							{
								iter->second.x()*=bestScale;
								iter->second.y()*=bestScale;
								iter->second.z()*=bestScale;
								if(outputPosesAlignedToGt)
									iter->second = bestGtToMap * iter->second;
							}

							// Scale/align odom poses
							for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
							{
								iter->second.x()*=bestScale;
								iter->second.y()*=bestScale;
								iter->second.z()*=bestScale;
								if(outputPosesAlignedToGt)
									iter->second = bestGtToOdom * iter->second;
							}

							if(outputRelativeError)
							{
								if(groundTruth.size() == poses.size())
								{
									// compute Motion statistics
									graph::calcRelativeErrors(uValues(groundTruth), uValues(poses), relative_t_err, relative_r_err);
								}
								else
								{
									std::vector<Transform> gtPosesTmp;
									std::vector<Transform> rPoses;
									for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
									{
										if(groundTruth.find(iter->first) != groundTruth.end())
										{
											gtPosesTmp.push_back(groundTruth.at(iter->first));
											rPoses.push_back(poses.at(iter->first));
										}
									}
									if(!gtPosesTmp.empty())
									{
										graph::calcRelativeErrors(gtPosesTmp, rPoses, relative_t_err, relative_r_err);
									}
								}
							}

							if(outputKittiError)
							{
								if(groundTruth.size() == poses.size())
								{
									// compute KITTI statistics
									graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), kitti_t_err, kitti_r_err);
								}
								else
								{
									printf("Cannot compute KITTI statistics as optimized poses and ground truth don't have the same size (%d vs %d).\n",
											(int)poses.size(), (int)groundTruth.size());
								}
							}

							if(outputPoses)
							{
								std::string dir = UDirectory::getDir(filePath);
								std::string dbName = UFile::getName(filePath);
								dbName = dbName.substr(0, dbName.size()-3); // remove db
								std::string path = dir+UDirectory::separator()+dbName+"_slam.txt";
								std::multimap<int, Link> dummyLinks;
								std::map<int, double> stamps;
								if(!outputKittiError)
								{
									// re-add landmarks
									uInsert(poses, optimizedLandmarks);
								}
								if(!outputKittiError)
								{
									for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
									{
										if(iter->first < 0)
										{
											stamps.insert(std::make_pair(iter->first, 0));
										}
										else
										{
											UASSERT(odomStamps.find(iter->first) != odomStamps.end());
											stamps.insert(*odomStamps.find(iter->first));
										}
									}
								}
								if(!graph::exportPoses(path, outputKittiError?2:11, poses, dummyLinks, stamps))
								{
									printf("Could not export the poses to \"%s\"!?!\n", path.c_str());
								}

								//export odom
								path = dir+UDirectory::separator()+dbName+"_odom.txt";
								stamps.clear();
								if(!outputKittiError)
								{
									for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
									{
										if(iter->first < 0)
										{
											stamps.insert(std::make_pair(iter->first, 0));
										}
										else
										{
											UASSERT(odomStamps.find(iter->first) != odomStamps.end());
											stamps.insert(*odomStamps.find(iter->first));
										}
									}
								}
								if(!graph::exportPoses(path, outputKittiError?2:11, posesOut, dummyLinks, stamps))
								{
									printf("Could not export the odometry to \"%s\"!?!\n", path.c_str());
								}

								//export ground truth
								if(groundTruth.size())
								{
									path = dir+UDirectory::separator()+dbName+"_gt.txt";
									stamps.clear();
									if(!outputKittiError)
									{
										for(std::map<int, Transform>::iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
										{
											UASSERT(odomStamps.find(iter->first) != odomStamps.end());
											stamps.insert(*odomStamps.find(iter->first));
										}
									}
									if(!graph::exportPoses(path, outputKittiError?2:11, groundTruth, dummyLinks, stamps))
									{
										printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
									}
								}
							}

							if (outputReport)
							{
								bool fillHeader = false;
								std::ifstream f("report.csv");
    							if(!f.good())
								{
									fillHeader = true;
								}

								std::ofstream myfile;
								myfile.open("report.csv", std::fstream::in | std::fstream::out | std::fstream::app);
								if(fillHeader)
								{
									myfile 	<< "Rosbag name"<<";"<<"error linear (m)"<<";"<<"error linear max (m)"<<";"<<"error linear odom (m)"<<";"
											<<"error angular"<<";"
											<<"Slam avg (hz)"<<";"<<"Slam max (hz)"<<";"
											<<"Odom avg (hz)"<<";"<<"Odom max (hz)"<<std::endl;
								}

								myfile 	<<fileName.c_str()<<";"
										<<bestRMSE<< ";"
										<<maxRMSE<<";"
										<<bestVoRMSE<<";"
										<<bestRMSEAng<<";"
										<<(1/(uMean(slamTime)/1000.0))<<";"
										<<(1/(uMax(slamTime)/1000.0))<<";"
										<<(1/(uMean(odomTime)/1000.0))<<";"
										<<(1/(uMax(odomTime)/1000.0))<<";"<<std::endl;
								myfile.close();
							}

							if(outputLoopAccuracy && !groundTruth.empty() && !linksOut.empty())
							{
								float sumDist = 0.0f;
								float sumAngle = 0.0f;
								int count = 0;
								for(std::multimap<int, Link>::iterator iter=loopClosureLinks.begin(); iter!=loopClosureLinks.end(); ++iter)
								{
									if(	groundTruth.find(iter->second.from())!=groundTruth.end() &&
										groundTruth.find(iter->second.to())!=groundTruth.end())
									{
										Transform gtLink = groundTruth.at(iter->second.from()).inverse()*groundTruth.at(iter->second.to());
										const Transform & t = iter->second.transform();
										Transform scaledLink(
												t.r11(), t.r12(), t.r13(), t.x()*bestScale,
												t.r21(), t.r22(), t.r23(), t.y()*bestScale,
												t.r31(), t.r32(), t.r33(), t.z()*bestScale);
										sumDist += gtLink.getDistance(scaledLink);
										sumAngle += gtLink.getAngle(scaledLink);
										++count;
									}
								}
								if(count>0)
								{
									loop_t_err = sumDist/float(count);
									loop_r_err = sumAngle/float(count);
									loop_r_err *= 180/CV_PI; // Rotation error (deg)
								}
							}
						}
					}
					printf("   %s (%d, %.1f m%s): RMSE= %.3f m (max=%s, odom=%.3f m) ang=%.1f deg%s%s, %s: avg=%d ms (max=%d ms) loops=%d%s%s%s%s%s%s\n",
							fileName.c_str(),
							(int)ids.size(),
							totalOdomDistance,
							bestScale != 1.0f?uFormat(", s=%.3f", bestScale).c_str():"",
							bestRMSE,
							maxRMSE!=-1?uFormat("%.3fm", maxRMSE).c_str():"NA",
							bestVoRMSE,
							bestRMSEAng,
							!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
							!outputRelativeError?"":uFormat(", Relative: t_err=%.3fm r_err=%.2f deg", relative_t_err, relative_r_err).c_str(),
							!localizationMultiStats.empty()?"loc":"slam",
							(int)uMean(slamTime), (int)uMax(slamTime),
							(int)loopClosureLinks.size(),
							landmarks==0?"":uFormat(", landmarks = %d", landmarks).c_str(),
							!outputLoopAccuracy?"":uFormat(" (t_err=%.3fm r_err=%.2f deg)", loop_t_err, loop_r_err).c_str(),
							odomTime.empty()?"":uFormat(", odom: avg=%dms (max=%dms)", (int)uMean(odomTime), (int)uMax(odomTime)).c_str(),
							cameraTime.empty()?"":uFormat(", camera: avg=%dms", (int)uMean(cameraTime)).c_str(),
							maxOdomRAM!=-1.0f?uFormat(", RAM odom=%dMB ", (int)maxOdomRAM).c_str():"",
							maxMapRAM!=-1.0f?uFormat(", map=%dMB",(int)maxMapRAM).c_str():"");

					if(outputLatex)
					{
						std::vector<float> stats;
						stats.push_back(ids.size());
						stats.push_back(bestRMSE);
						stats.push_back(maxRMSE);
						stats.push_back(bestRMSEAng);
						stats.push_back(uMean(odomTime));
						stats.push_back(uMean(slamTime));
						stats.push_back(uMax(slamTime));
						stats.push_back(maxOdomRAM);
						stats.push_back(maxMapRAM);
						outputLatexStatisticsMap.insert(std::make_pair(filePath, stats));

						if(maxOdomRAM != -1.0f)
						{
							odomRAMSet = true;
						}
					}
				}
				driver->closeConnection();
				delete driver;
			}
			else if(uSplit(fileName, '.').size() == 1)
			{
				//sub directory
				subDirs.push_front(currentPath + UDirectory::separator() + fileName);
			}
			currentPathIsDatabase = false;
		}

		if(!localizationMultiStats.empty() && showLoc!=-1)
		{
			printf("---Localization results---\n");
			std::string prefix = "header={";
			printf("%s", prefix.c_str());
			for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator iter=localizationMultiStats.begin()->second.begin();
						iter!=localizationMultiStats.begin()->second.end();)
			{
				if(iter!=localizationMultiStats.begin()->second.begin())
				{
					printf("%s",  std::string(prefix.size(), ' ').c_str());
				}
				printf("%s", iter->first.c_str());
				++iter;
				if(iter!=localizationMultiStats.begin()->second.end())
				{
					printf(";\n");
				}
			}
			printf("}\n");


			for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator iter=localizationMultiStats.begin();
				iter!=localizationMultiStats.end();
				++iter)
			{
				printf("%s\n", iter->first.c_str());
				for(int k=0; k<6; ++k)
				{
					if(showLoc & (0x1 << k))
					{
						std::string prefix = uFormat("  %s=[",
								k==0?"min":
								k==1?"max":
								k==2?"mean":
								k==3?"stddev":
								k==4?"total":
								"nonnull%");
						printf("%s", prefix.c_str());
						for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator jter=iter->second.begin(); jter!=iter->second.end();)
						{
							if(jter!=iter->second.begin())
							{
								printf("%s", std::string(prefix.size(), ' ').c_str());
							}
							for(size_t j=0; j<jter->second.size(); ++j)
							{
								if(k<4)
								{
									printf("%f",
											k==0?jter->second[j].min:
											k==1?jter->second[j].max:
											k==2?jter->second[j].mean:
											jter->second[j].stddev);
								}
								else if(k==4)
								{
									printf("%d",jter->second[j].total);
								}
								else if(k==5)
								{
									printf("%.2f", (jter->second[j].nonNull*100));
								}
								if(j+1 < jter->second.size())
								{
									printf(" ");
								}
							}
							++jter;
							if(jter!=iter->second.end())
							{
								printf(";\n");
							}
						}
						printf("]\n");
					}
				}
				iter->second.clear();
			}
		}

		for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
		{
			paths.push_front(*iter);
		}

		if(outputLatexStatisticsMap.size() && paths.empty())
		{
			outputLatexStatistics.push_back(outputLatexStatisticsMap);
			outputLatexStatisticsMap.clear();
		}
	}

	if(outputLatex && outputLatexStatistics.size())
	{
		printf("\nLaTeX output:\n----------------\n");
		printf("\\begin{table*}[!t]\n");
		printf("\\caption{$t_{end}$ is the absolute translational RMSE value at the end "
				"of the experiment as $ATE_{max}$ is the maximum during the experiment. "
				"$r_{end}$ is rotational RMSE value at the end of the experiment. "
				"$o_{avg}$ and $m_{avg}$ are the average computational time "
				"for odometry (front-end) and map update (back-end). "
				"$m_{avg}$ is the maximum computational time for map update. "
				"$O_{end}$ and $M_{end}$ are the RAM usage at the end of the experiment "
				"for odometry and map management respectively.}\n");
		printf("\\label{}\n");
		printf("\\centering\n");
		if(odomRAMSet)
		{
			printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c|c}\n");
			printf("\\cline{2-10}\n");
			printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $O_{end}$ & $M_{end}$  \\\\\n");
			printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) & (MB) \\\\\n");
		}
		else
		{
			printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c}\n");
			printf("\\cline{2-9}\n");
			printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $M_{end}$  \\\\\n");
			printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB)  \\\\\n");
		}

		printf("\\hline\n");

		for(unsigned int j=0; j<outputLatexStatistics.size(); ++j)
		{
			if(outputLatexStatistics[j].size())
			{
				std::vector<int> lowestIndex;
				if(outputLatexStatistics[j].size() > 1)
				{
					std::vector<float> lowestValue(outputLatexStatistics[j].begin()->second.size(),-1);
					lowestIndex = std::vector<int>(lowestValue.size(),0);
					int index = 0;
					for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
					{
						UASSERT(lowestValue.size() == iter->second.size());
						for(unsigned int i=0; i<iter->second.size(); ++i)
						{
							if(lowestValue[i] == -1 || (iter->second[i]>0.0f && lowestValue[i]>iter->second[i]))
							{
								lowestValue[i] = iter->second[i];
								lowestIndex[i] = index;
							}
						}
						++index;
					}
				}

				int index = 0;
				for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
				{
					UASSERT(iter->second.size() == 9);
					printf("%s & ", uReplaceChar(iter->first.c_str(), '_', '-').c_str());
					printf("%d & ", (int)iter->second[0]);
					printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[1]==index?"\\textbf{":"", iter->second[1], lowestIndex.size()&&lowestIndex[1]==index?"}":"");
					printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[2]==index?"\\textbf{":"", iter->second[2], lowestIndex.size()&&lowestIndex[2]==index?"}":"");
					printf("%s%.2f%s & ", lowestIndex.size()&&lowestIndex[3]==index?"\\textbf{":"", iter->second[3], lowestIndex.size()&&lowestIndex[3]==index?"}":"");
					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[4]==index?"\\textbf{":"", (int)iter->second[4], lowestIndex.size()&&lowestIndex[4]==index?"}":"");
					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[5]==index?"\\textbf{":"", (int)iter->second[5], lowestIndex.size()&&lowestIndex[5]==index?"}":"");
					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[6]==index?"\\textbf{":"", (int)iter->second[6], lowestIndex.size()&&lowestIndex[6]==index?"}":"");
					if(odomRAMSet)
					{
						printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[7]==index?"\\textbf{":"", (int)iter->second[7], lowestIndex.size()&&lowestIndex[7]==index?"}":"");
					}
					printf("%s%d%s ", lowestIndex.size()&&lowestIndex[8]==index?"\\textbf{":"", (int)iter->second[8], lowestIndex.size()&&lowestIndex[8]==index?"}":"");
					printf("\\\\\n");
					++index;
				}
				printf("\\hline\n");
			}
		}


		printf("\\end{tabular}\n");
		printf("\\end{table*}\n----------------\n");
	}
#ifdef WITH_QT
	if(figures.size())
	{
		for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
		{
			if(!useIds)
			{
				iter->second->frameData();
			}
			if(exportFigures)
			{
				QString data = iter->second->getAllCurveDataAsText();
				if(!data.isEmpty())
				{
					QString filePath = QString(exportPrefix.c_str()) + (exportPrefix.empty()?"":"-") + iter->second->windowTitle().replace('/', "-") + ".txt";
					QFile file(filePath);
					if(file.open(QIODevice::Text | QIODevice::WriteOnly))
					{
						file.write(data.toUtf8());
						file.close();
						printf("Exported \"%s\".\n", filePath.toStdString().c_str());
					}
					else
					{
						printf("ERROR: could not open file \"%s\" for writing!\n", filePath.toStdString().c_str());
					}
				}
			}
			else
			{
				iter->second->show();
			}
		}
		if(!exportFigures)
		{
			return app.exec();
		}
	}
#endif
	return 0;
}