main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap/core/DBDriver.h>
00029 #include <rtabmap/core/Graph.h>
00030 #include <rtabmap/core/Optimizer.h>
00031 #include <rtabmap/utilite/UDirectory.h>
00032 #include <rtabmap/utilite/UFile.h>
00033 #include <rtabmap/utilite/UStl.h>
00034 #include <rtabmap/utilite/UMath.h>
00035 #include <rtabmap/utilite/UPlot.h>
00036 #include <rtabmap/utilite/ULogger.h>
00037 #include <QApplication>
00038 #include <stdio.h>
00039 
00040 using namespace rtabmap;
00041 
00042 void showUsage()
00043 {
00044         printf("\nUsage:\n"
00045                         "rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] [--poses] path\n"
00046                         "  path               Directory containing rtabmap databases or path of a database.\n"
00047                         "  --latex            Print table formatted in LaTeX with results.\n"
00048                         "  --kitti            Compute error based on KITTI benchmark.\n"
00049                         "  --scale            Find the best scale for the map against the ground truth\n"
00050                         "                       and compute error based on the scaled path.\n"
00051                         "  --poses            Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n"
00052                         "                       and valid ground truth indices to [path]_indices.txt \n\n");
00053         exit(1);
00054 }
00055 
00056 int main(int argc, char * argv[])
00057 {
00058         if(argc < 2)
00059         {
00060                 showUsage();
00061         }
00062 
00063         ULogger::setType(ULogger::kTypeConsole);
00064         ULogger::setLevel(ULogger::kWarning);
00065 
00066         QApplication app(argc, argv);
00067 
00068         bool outputLatex = false;
00069         bool outputScaled = false;
00070         bool outputPoses = false;
00071         bool outputKittiError = false;
00072         std::map<std::string, UPlot*> figures;
00073         for(int i=1; i<argc-1; ++i)
00074         {
00075                 if(strcmp(argv[i], "--latex") == 0)
00076                 {
00077                         outputLatex = true;
00078                 }
00079                 else if(strcmp(argv[i], "--kitti") == 0)
00080                 {
00081                         outputKittiError = true;
00082                 }
00083                 else if(strcmp(argv[i], "--scale") == 0)
00084                 {
00085                         outputScaled = true;
00086                 }
00087                 else if(strcmp(argv[i], "--poses") == 0)
00088                 {
00089                         outputPoses = true;
00090                 }
00091                 else
00092                 {
00093                         std::string figureTitle = argv[i];
00094                         printf("Plot %s\n", figureTitle.c_str());
00095                         UPlot * fig = new UPlot();
00096                         fig->setTitle(figureTitle.c_str());
00097                         fig->setXLabel("Time (s)");
00098                         figures.insert(std::make_pair(figureTitle, fig));
00099                 }
00100         }
00101 
00102         std::string path = argv[argc-1];
00103         path = uReplaceChar(path, '~', UDirectory::homeDir());
00104 
00105         std::string fileName;
00106         std::list<std::string> paths;
00107         paths.push_back(path);
00108         std::vector<std::map<std::string, std::vector<float> > > outputLatexStatistics;
00109         std::map<std::string, std::vector<float> > outputLatexStatisticsMap;
00110         bool odomRAMSet = false;
00111         std::set<std::string> topDirs;
00112         while(paths.size())
00113         {
00114                 std::string currentPath = paths.front();
00115                 UDirectory currentDir(currentPath);
00116                 paths.pop_front();
00117                 bool currentPathIsDatabase = false;
00118                 if(!currentDir.isValid())
00119                 {
00120                         if(UFile::getExtension(currentPath).compare("db") == 0)
00121                         {
00122                                 currentPathIsDatabase=true;
00123                                 printf("Database: %s\n", currentPath.c_str());
00124                         }
00125                         else
00126                         {
00127                                 continue;
00128                         }
00129                 }
00130                 std::list<std::string> subDirs;
00131                 if(!currentPathIsDatabase)
00132                 {
00133                         printf("Directory: %s\n", currentPath.c_str());
00134                         std::list<std::string> fileNames = currentDir.getFileNames();
00135                         if(topDirs.empty())
00136                         {
00137                                 for(std::list<std::string>::iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
00138                                 {
00139                                         topDirs.insert(currentPath+"/"+*iter);
00140                                 }
00141                         }
00142                         else
00143                         {
00144                                 if(topDirs.find(currentPath) != topDirs.end())
00145                                 {
00146                                         if(outputLatexStatisticsMap.size())
00147                                         {
00148                                                 outputLatexStatistics.push_back(outputLatexStatisticsMap);
00149                                                 outputLatexStatisticsMap.clear();
00150                                         }
00151                                 }
00152                         }
00153                 }
00154 
00155                 while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
00156                 {
00157                         if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
00158                         {
00159                                 std::string filePath;
00160                                 if(currentPathIsDatabase)
00161                                 {
00162                                         filePath = currentPath;
00163                                 }
00164                                 else
00165                                 {
00166                                         filePath = currentPath + UDirectory::separator() + fileName;
00167                                 }
00168 
00169                                 DBDriver * driver = DBDriver::create();
00170                                 ParametersMap params;
00171                                 if(driver->openConnection(filePath))
00172                                 {
00173                                         ULogger::setLevel(ULogger::kError); // to suppress parameter warnings
00174                                         params = driver->getLastParameters();
00175                                         ULogger::setLevel(ULogger::kWarning);
00176                                         std::set<int> ids;
00177                                         driver->getAllNodeIds(ids);
00178                                         std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
00179                                         std::map<int, Transform> odomPoses, gtPoses;
00180                                         std::vector<float> cameraTime;
00181                                         cameraTime.reserve(ids.size());
00182                                         std::vector<float> odomTime;
00183                                         odomTime.reserve(ids.size());
00184                                         std::vector<float> slamTime;
00185                                         slamTime.reserve(ids.size());
00186                                         float rmse = -1;
00187                                         float maxRMSE = -1;
00188                                         float maxOdomRAM = -1;
00189                                         float maxMapRAM = -1;
00190                                         std::map<std::string, UPlotCurve*> curves;
00191                                         std::map<std::string, double> firstStamps;
00192                                         for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
00193                                         {
00194                                                 curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
00195                                         }
00196 
00197                                         for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
00198                                         {
00199                                                 Transform p, gt;
00200                                                 GPS gps;
00201                                                 int m=-1, w=-1;
00202                                                 std::string l;
00203                                                 double s;
00204                                                 std::vector<float> v;
00205                                                 if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps))
00206                                                 {
00207                                                         odomPoses.insert(std::make_pair(*iter, p));
00208                                                         if(!gt.isNull())
00209                                                         {
00210                                                                 gtPoses.insert(std::make_pair(*iter, gt));
00211                                                         }
00212                                                         if(uContains(stats, *iter))
00213                                                         {
00214                                                                 const std::map<std::string, float> & stat = stats.at(*iter).first;
00215                                                                 if(uContains(stat, Statistics::kGtTranslational_rmse()))
00216                                                                 {
00217                                                                         rmse = stat.at(Statistics::kGtTranslational_rmse());
00218                                                                         if(maxRMSE==-1 || maxRMSE < rmse)
00219                                                                         {
00220                                                                                 maxRMSE = rmse;
00221                                                                         }
00222                                                                 }
00223                                                                 if(uContains(stat, std::string("Camera/TotalTime/ms")))
00224                                                                 {
00225                                                                         cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
00226                                                                 }
00227                                                                 if(uContains(stat, std::string("Odometry/TotalTime/ms")))
00228                                                                 {
00229                                                                         odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
00230                                                                 }
00231 
00232                                                                 if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
00233                                                                 {
00234                                                                         if(w>=0 || stat.at("RtabmapROS/TotalTime/ms") > 10.0f)
00235                                                                         {
00236                                                                                 slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
00237                                                                         }
00238                                                                 }
00239                                                                 else if(uContains(stat, Statistics::kTimingTotal()))
00240                                                                 {
00241                                                                         if(w>=0 || stat.at(Statistics::kTimingTotal()) > 10.0f)
00242                                                                         {
00243                                                                                 slamTime.push_back(stat.at(Statistics::kTimingTotal()));
00244                                                                         }
00245                                                                 }
00246 
00247                                                                 if(uContains(stat, std::string(Statistics::kMemoryRAM_usage())))
00248                                                                 {
00249                                                                         float ram = stat.at(Statistics::kMemoryRAM_usage());
00250                                                                         if(maxMapRAM==-1 || maxMapRAM < ram)
00251                                                                         {
00252                                                                                 maxMapRAM = ram;
00253                                                                         }
00254                                                                 }
00255                                                                 if(uContains(stat, std::string("Odometry/RAM_usage/MB")))
00256                                                                 {
00257                                                                         float ram = stat.at("Odometry/RAM_usage/MB");
00258                                                                         if(maxOdomRAM==-1 || maxOdomRAM < ram)
00259                                                                         {
00260                                                                                 maxOdomRAM = ram;
00261                                                                         }
00262                                                                 }
00263 
00264                                                                 for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
00265                                                                 {
00266                                                                         if(uContains(stat, jter->first))
00267                                                                         {
00268                                                                                 if(!uContains(firstStamps, jter->first))
00269                                                                                 {
00270                                                                                         firstStamps.insert(std::make_pair(jter->first, s));
00271                                                                                 }
00272                                                                                 float x = s - firstStamps.at(jter->first);
00273                                                                                 float y = stat.at(jter->first);
00274                                                                                 jter->second->addValue(x,y);
00275                                                                         }
00276                                                                 }
00277                                                         }
00278                                                 }
00279                                         }
00280 
00281                                         std::multimap<int, Link> links;
00282                                         driver->getAllLinks(links, true);
00283                                         std::multimap<int, Link> loopClosureLinks;
00284                                         for(std::multimap<int, Link>::iterator jter=links.begin(); jter!=links.end(); ++jter)
00285                                         {
00286                                                 if(jter->second.type() == Link::kGlobalClosure &&
00287                                                         graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end())
00288                                                 {
00289                                                         loopClosureLinks.insert(*jter);
00290                                                 }
00291                                         }
00292 
00293                                         float bestScale = 1.0f;
00294                                         float bestRMSE = -1;
00295                                         float bestRMSEAng = -1;
00296                                         float bestVoRMSE = -1;
00297                                         Transform bestGtToMap = Transform::getIdentity();
00298                                         float kitti_t_err = 0.0f;
00299                                         float kitti_r_err = 0.0f;
00300                                         if(ids.size())
00301                                         {
00302                                                 std::map<int, Transform> posesOut;
00303                                                 std::multimap<int, Link> linksOut;
00304                                                 int firstId = *ids.begin();
00305                                                 rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
00306                                                 optimizer->getConnectedGraph(firstId, odomPoses, graph::filterDuplicateLinks(links), posesOut, linksOut);
00307 
00308                                                 std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut);
00309                                                 if(poses.empty())
00310                                                 {
00311                                                         // try incremental optimization
00312                                                         UWARN("Optimization failed! Try incremental optimization...");
00313                                                         poses = optimizer->optimizeIncremental(firstId, posesOut, linksOut);
00314                                                         if(poses.empty())
00315                                                         {
00316                                                                 UERROR("Incremental optimization also failed! Only original RMSE will be shown.");
00317                                                                 bestRMSE = rmse;
00318                                                         }
00319                                                         else
00320                                                         {
00321                                                                 UWARN("Incremental optimization succeeded!");
00322                                                         }
00323                                                 }
00324 
00325                                                 if(poses.size())
00326                                                 {
00327                                                         std::map<int, Transform> groundTruth;
00328                                                         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00329                                                         {
00330                                                                 if(gtPoses.find(iter->first) != gtPoses.end())
00331                                                                 {
00332                                                                         groundTruth.insert(*gtPoses.find(iter->first));
00333                                                                 }
00334                                                         }
00335 
00336                                                         outputScaled = outputScaled && groundTruth.size();
00337                                                         for(float scale=outputScaled?0.900f:1.0f; scale<1.100f; scale+=0.001)
00338                                                         {
00339                                                                 std::map<int, Transform> scaledPoses;
00340                                                                 std::map<int, Transform> scaledOdomPoses;
00341 
00342                                                                 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00343                                                                 {
00344                                                                         Transform t = iter->second.clone();
00345                                                                         t.x() *= scale;
00346                                                                         t.y() *= scale;
00347                                                                         t.z() *= scale;
00348                                                                         scaledPoses.insert(std::make_pair(iter->first, t));
00349 
00350                                                                         UASSERT(posesOut.find(iter->first)!=posesOut.end());
00351                                                                         t = posesOut.at(iter->first).clone();
00352                                                                         t.x() *= scale;
00353                                                                         t.y() *= scale;
00354                                                                         t.z() *= scale;
00355                                                                         scaledOdomPoses.insert(std::make_pair(iter->first, t));
00356                                                                 }
00357                                                                 // compute RMSE statistics
00358                                                                 float translational_rmse = 0.0f;
00359                                                                 float translational_mean = 0.0f;
00360                                                                 float translational_median = 0.0f;
00361                                                                 float translational_std = 0.0f;
00362                                                                 float translational_min = 0.0f;
00363                                                                 float translational_max = 0.0f;
00364                                                                 float rotational_rmse = 0.0f;
00365                                                                 float rotational_mean = 0.0f;
00366                                                                 float rotational_median = 0.0f;
00367                                                                 float rotational_std = 0.0f;
00368                                                                 float rotational_min = 0.0f;
00369                                                                 float rotational_max = 0.0f;
00370 
00371                                                                 graph::calcRMSE(
00372                                                                                 groundTruth,
00373                                                                                 scaledOdomPoses,
00374                                                                                 translational_rmse,
00375                                                                                 translational_mean,
00376                                                                                 translational_median,
00377                                                                                 translational_std,
00378                                                                                 translational_min,
00379                                                                                 translational_max,
00380                                                                                 rotational_rmse,
00381                                                                                 rotational_mean,
00382                                                                                 rotational_median,
00383                                                                                 rotational_std,
00384                                                                                 rotational_min,
00385                                                                                 rotational_max);
00386                                                                 float translational_rmse_vo = translational_rmse;
00387 
00388                                                                 Transform gtToMap = graph::calcRMSE(
00389                                                                                 groundTruth,
00390                                                                                 scaledPoses,
00391                                                                                 translational_rmse,
00392                                                                                 translational_mean,
00393                                                                                 translational_median,
00394                                                                                 translational_std,
00395                                                                                 translational_min,
00396                                                                                 translational_max,
00397                                                                                 rotational_rmse,
00398                                                                                 rotational_mean,
00399                                                                                 rotational_median,
00400                                                                                 rotational_std,
00401                                                                                 rotational_min,
00402                                                                                 rotational_max);
00403 
00404                                                                 if(bestRMSE!=-1 && translational_rmse > bestRMSE)
00405                                                                 {
00406                                                                         break;
00407                                                                 }
00408                                                                 bestRMSE = translational_rmse;
00409                                                                 bestVoRMSE = translational_rmse_vo;
00410                                                                 bestRMSEAng = rotational_rmse;
00411                                                                 bestScale = scale;
00412                                                                 bestGtToMap = gtToMap;
00413                                                                 if(!outputScaled)
00414                                                                 {
00415                                                                         // just did iteration without any scale, then exit
00416                                                                         break;
00417                                                                 }
00418                                                         }
00419 
00420                                                         for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00421                                                         {
00422                                                                 iter->second.x()*=bestScale;
00423                                                                 iter->second.y()*=bestScale;
00424                                                                 iter->second.z()*=bestScale;
00425                                                                 iter->second = bestGtToMap * iter->second;
00426                                                         }
00427 
00428                                                         if(outputKittiError)
00429                                                         {
00430                                                                 if(groundTruth.size() == poses.size())
00431                                                                 {
00432                                                                         // compute KITTI statistics
00433                                                                         graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), kitti_t_err, kitti_r_err);
00434                                                                 }
00435                                                                 else
00436                                                                 {
00437                                                                         printf("Cannot compute KITTI statistics as optimized poses and ground truth don't have the same size (%d vs %d).\n",
00438                                                                                         (int)poses.size(), (int)groundTruth.size());
00439                                                                 }
00440                                                         }
00441 
00442                                                         if(outputPoses)
00443                                                         {
00444                                                                 std::string dir = UDirectory::getDir(filePath);
00445                                                                 std::string dbName = UFile::getName(filePath);
00446                                                                 dbName = dbName.substr(0, dbName.size()-3); // remove db
00447                                                                 std::string path = dir+UDirectory::separator()+dbName+"_poses.txt";
00448                                                                 if(!graph::exportPoses(path, outputKittiError?2:0, poses))
00449                                                                 {
00450                                                                         printf("Could not export the poses to \"%s\"!?!\n", path.c_str());
00451                                                                 }
00452                                                                 if(groundTruth.size())
00453                                                                 {
00454                                                                         // For missing ground truth poses, set them to null
00455                                                                         std::vector<int> validIndices(poses.size(), 1);
00456                                                                         int i=0;
00457                                                                         for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter, ++i)
00458                                                                         {
00459                                                                                 if(groundTruth.find(iter->first) == groundTruth.end())
00460                                                                                 {
00461                                                                                         groundTruth.insert(std::make_pair(iter->first, Transform()));
00462                                                                                         validIndices[i] = 0;
00463                                                                                 }
00464                                                                         }
00465                                                                         path = dir+UDirectory::separator()+dbName+"_gt.txt";
00466                                                                         if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth))
00467                                                                         {
00468                                                                                 printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
00469                                                                         }
00470                                                                         else
00471                                                                         {
00472                                                                                 // save valid indices
00473                                                                                 path = dir+UDirectory::separator()+dbName+"_indices.txt";
00474                                                                                 FILE * file = 0;
00475         #ifdef _MSC_VER
00476                                                                                 fopen_s(&file, path.c_str(), "w");
00477         #else
00478                                                                                 file = fopen(path.c_str(), "w");
00479         #endif
00480                                                                                 if(file)
00481                                                                                 {
00482                                                                                         // VERTEX3 id x y z phi theta psi
00483                                                                                         for(unsigned int k=0; k<validIndices.size(); ++k)
00484                                                                                         {
00485                                                                                                 fprintf(file, "%d\n", validIndices[k]);
00486                                                                                         }
00487                                                                                         fclose(file);
00488                                                                                 }
00489                                                                         }
00490                                                                 }
00491                                                         }
00492                                                 }
00493                                         }
00494                                         printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
00495                                                         fileName.c_str(),
00496                                                         (int)ids.size(),
00497                                                         bestScale,
00498                                                         bestRMSE,
00499                                                         maxRMSE,
00500                                                         bestVoRMSE,
00501                                                         bestRMSEAng,
00502                                                         !outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
00503                                                         (int)uMean(slamTime), (int)uMax(slamTime),
00504                                                         (int)loopClosureLinks.size(),
00505                                                         (int)uMean(odomTime), (int)uMax(odomTime),
00506                                                         (int)uMean(cameraTime),
00507                                                         maxOdomRAM!=-1.0f?uFormat("RAM odom=%dMB ", (int)maxOdomRAM).c_str():"",
00508                                                         (int)maxMapRAM);
00509 
00510                                         if(outputLatex)
00511                                         {
00512                                                 std::vector<float> stats;
00513                                                 stats.push_back(ids.size());
00514                                                 stats.push_back(bestRMSE);
00515                                                 stats.push_back(maxRMSE);
00516                                                 stats.push_back(bestRMSEAng);
00517                                                 stats.push_back(uMean(odomTime));
00518                                                 stats.push_back(uMean(slamTime));
00519                                                 stats.push_back(uMax(slamTime));
00520                                                 stats.push_back(maxOdomRAM);
00521                                                 stats.push_back(maxMapRAM);
00522                                                 outputLatexStatisticsMap.insert(std::make_pair(filePath, stats));
00523 
00524                                                 if(maxOdomRAM != -1.0f)
00525                                                 {
00526                                                         odomRAMSet = true;
00527                                                 }
00528                                         }
00529                                 }
00530                                 driver->closeConnection();
00531                                 delete driver;
00532                         }
00533                         else if(uSplit(fileName, '.').size() == 1)
00534                         {
00535                                 //sub directory
00536                                 subDirs.push_front(currentPath + UDirectory::separator() + fileName);
00537                         }
00538                         currentPathIsDatabase = false;
00539                 }
00540 
00541                 for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
00542                 {
00543                         paths.push_front(*iter);
00544                 }
00545 
00546                 if(outputLatexStatisticsMap.size() && paths.empty())
00547                 {
00548                         outputLatexStatistics.push_back(outputLatexStatisticsMap);
00549                         outputLatexStatisticsMap.clear();
00550                 }
00551         }
00552 
00553         if(outputLatex && outputLatexStatistics.size())
00554         {
00555                 printf("\nLaTeX output:\n----------------\n");
00556                 printf("\\begin{table*}[!t]\n");
00557                 printf("\\caption{$t_{end}$ is the absolute translational RMSE value at the end "
00558                                 "of the experiment as $ATE_{max}$ is the maximum during the experiment. "
00559                                 "$r_{end}$ is rotational RMSE value at the end of the experiment. "
00560                                 "$o_{avg}$ and $m_{avg}$ are the average computational time "
00561                                 "for odometry (front-end) and map update (back-end). "
00562                                 "$m_{avg}$ is the maximum computational time for map update. "
00563                                 "$O_{end}$ and $M_{end}$ are the RAM usage at the end of the experiment "
00564                                 "for odometry and map management respectively.}\n");
00565                 printf("\\label{}\n");
00566                 printf("\\centering\n");
00567                 if(odomRAMSet)
00568                 {
00569                         printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c|c}\n");
00570                         printf("\\cline{2-10}\n");
00571                         printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $O_{end}$ & $M_{end}$  \\\\\n");
00572                         printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) & (MB) \\\\\n");
00573                 }
00574                 else
00575                 {
00576                         printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c}\n");
00577                         printf("\\cline{2-9}\n");
00578                         printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $M_{end}$  \\\\\n");
00579                         printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB)  \\\\\n");
00580                 }
00581 
00582                 printf("\\hline\n");
00583 
00584                 for(unsigned int j=0; j<outputLatexStatistics.size(); ++j)
00585                 {
00586                         if(outputLatexStatistics[j].size())
00587                         {
00588                                 std::vector<int> lowestIndex;
00589                                 if(outputLatexStatistics[j].size() > 1)
00590                                 {
00591                                         std::vector<float> lowestValue(outputLatexStatistics[j].begin()->second.size(),-1);
00592                                         lowestIndex = std::vector<int>(lowestValue.size(),0);
00593                                         int index = 0;
00594                                         for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
00595                                         {
00596                                                 UASSERT(lowestValue.size() == iter->second.size());
00597                                                 for(unsigned int i=0; i<iter->second.size(); ++i)
00598                                                 {
00599                                                         if(lowestValue[i] == -1 || (iter->second[i]>0.0f && lowestValue[i]>iter->second[i]))
00600                                                         {
00601                                                                 lowestValue[i] = iter->second[i];
00602                                                                 lowestIndex[i] = index;
00603                                                         }
00604                                                 }
00605                                                 ++index;
00606                                         }
00607                                 }
00608 
00609                                 int index = 0;
00610                                 for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
00611                                 {
00612                                         UASSERT(iter->second.size() == 9);
00613                                         printf("%s & ", uReplaceChar(iter->first.c_str(), '_', '-').c_str());
00614                                         printf("%d & ", (int)iter->second[0]);
00615                                         printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[1]==index?"\\textbf{":"", iter->second[1], lowestIndex.size()&&lowestIndex[1]==index?"}":"");
00616                                         printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[2]==index?"\\textbf{":"", iter->second[2], lowestIndex.size()&&lowestIndex[2]==index?"}":"");
00617                                         printf("%s%.2f%s & ", lowestIndex.size()&&lowestIndex[3]==index?"\\textbf{":"", iter->second[3], lowestIndex.size()&&lowestIndex[3]==index?"}":"");
00618                                         printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[4]==index?"\\textbf{":"", (int)iter->second[4], lowestIndex.size()&&lowestIndex[4]==index?"}":"");
00619                                         printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[5]==index?"\\textbf{":"", (int)iter->second[5], lowestIndex.size()&&lowestIndex[5]==index?"}":"");
00620                                         printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[6]==index?"\\textbf{":"", (int)iter->second[6], lowestIndex.size()&&lowestIndex[6]==index?"}":"");
00621                                         if(odomRAMSet)
00622                                         {
00623                                                 printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[7]==index?"\\textbf{":"", (int)iter->second[7], lowestIndex.size()&&lowestIndex[7]==index?"}":"");
00624                                         }
00625                                         printf("%s%d%s ", lowestIndex.size()&&lowestIndex[8]==index?"\\textbf{":"", (int)iter->second[8], lowestIndex.size()&&lowestIndex[8]==index?"}":"");
00626                                         printf("\\\\\n");
00627                                         ++index;
00628                                 }
00629                                 printf("\\hline\n");
00630                         }
00631                 }
00632 
00633                 printf("\\end{tabular}\n");
00634                 printf("\\end{table*}\n----------------\n");
00635         }
00636 
00637         if(figures.size())
00638         {
00639                 for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
00640                 {
00641                         iter->second->show();
00642                 }
00643                 return app.exec();
00644         }
00645         return 0;
00646 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20