MainWindow.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/gui/MainWindow.h"
00029 
00030 #include "ui_mainWindow.h"
00031 
00032 #include "rtabmap/core/CameraRGB.h"
00033 #include "rtabmap/core/CameraStereo.h"
00034 #include "rtabmap/core/CameraThread.h"
00035 #include "rtabmap/core/CameraEvent.h"
00036 #include "rtabmap/core/DBReader.h"
00037 #include "rtabmap/core/Parameters.h"
00038 #include "rtabmap/core/ParamEvent.h"
00039 #include "rtabmap/core/Signature.h"
00040 #include "rtabmap/core/Memory.h"
00041 #include "rtabmap/core/DBDriver.h"
00042 #include "rtabmap/core/RegistrationVis.h"
00043 
00044 #include "rtabmap/gui/ImageView.h"
00045 #include "rtabmap/gui/KeypointItem.h"
00046 #include "rtabmap/gui/DataRecorder.h"
00047 #include "rtabmap/gui/DatabaseViewer.h"
00048 #include "rtabmap/gui/PdfPlot.h"
00049 #include "rtabmap/gui/StatsToolBox.h"
00050 #include "rtabmap/gui/ProgressDialog.h"
00051 #include "rtabmap/gui/CloudViewer.h"
00052 #include "rtabmap/gui/LoopClosureViewer.h"
00053 
00054 #include <rtabmap/utilite/UStl.h>
00055 #include <rtabmap/utilite/ULogger.h>
00056 #include <rtabmap/utilite/UEventsManager.h>
00057 #include <rtabmap/utilite/UFile.h>
00058 #include <rtabmap/utilite/UConversion.h>
00059 #include "rtabmap/utilite/UPlot.h"
00060 #include "rtabmap/utilite/UCv2Qt.h"
00061 
00062 #include "ExportCloudsDialog.h"
00063 #include "ExportScansDialog.h"
00064 #include "AboutDialog.h"
00065 #include "PostProcessingDialog.h"
00066 
00067 #include <QtGui/QCloseEvent>
00068 #include <QtGui/QPixmap>
00069 #include <QtCore/QDir>
00070 #include <QtCore/QFile>
00071 #include <QtCore/QTextStream>
00072 #include <QtCore/QFileInfo>
00073 #include <QMessageBox>
00074 #include <QFileDialog>
00075 #include <QGraphicsEllipseItem>
00076 #include <QDockWidget>
00077 #include <QtCore/QBuffer>
00078 #include <QtCore/QTimer>
00079 #include <QtCore/QTime>
00080 #include <QActionGroup>
00081 #include <QtCore/QThread>
00082 #include <QtGui/QDesktopServices>
00083 #include <QtCore/QStringList>
00084 #include <QtCore/QProcess>
00085 #include <QSplashScreen>
00086 #include <QInputDialog>
00087 #include <QToolButton>
00088 
00089 //RGB-D stuff
00090 #include "rtabmap/core/CameraRGBD.h"
00091 #include "rtabmap/core/Odometry.h"
00092 #include "rtabmap/core/OdometryThread.h"
00093 #include "rtabmap/core/OdometryEvent.h"
00094 #include "rtabmap/core/util3d.h"
00095 #include "rtabmap/core/util3d_transforms.h"
00096 #include "rtabmap/core/util3d_filtering.h"
00097 #include "rtabmap/core/util3d_mapping.h"
00098 #include "rtabmap/core/util3d_surface.h"
00099 #include "rtabmap/core/util3d_registration.h"
00100 #include "rtabmap/core/Optimizer.h"
00101 #include "rtabmap/core/OptimizerCVSBA.h"
00102 #include "rtabmap/core/Graph.h"
00103 #include "rtabmap/core/RegistrationIcp.h"
00104 #include <pcl/visualization/cloud_viewer.h>
00105 #include <pcl/common/transforms.h>
00106 #include <pcl/common/common.h>
00107 #include <pcl/io/pcd_io.h>
00108 #include <pcl/io/ply_io.h>
00109 #include <pcl/filters/filter.h>
00110 #include <pcl/search/kdtree.h>
00111 
00112 #ifdef RTABMAP_OCTOMAP
00113 #include <rtabmap/core/OctoMap.h>
00114 #endif
00115 
00116 #define LOG_FILE_NAME "LogRtabmap.txt"
00117 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
00118 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
00119 #define SHARE_IMPORT_FILE   "share/rtabmap/importfile.m"
00120 
00121 using namespace rtabmap;
00122 
00123 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
00124 
00125 
00126 namespace rtabmap {
00127 
00128 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
00129         QMainWindow(parent),
00130         _ui(0),
00131         _state(kIdle),
00132         _camera(0),
00133         _odomThread(0),
00134         _preferencesDialog(0),
00135         _aboutDialog(0),
00136         _exportCloudsDialog(0),
00137         _exportScansDialog(0),
00138         _dataRecorder(0),
00139         _lastId(0),
00140         _firstStamp(0.0f),
00141         _processingStatistics(false),
00142         _processingDownloadedMap(false),
00143         _odometryReceived(false),
00144         _newDatabasePath(""),
00145         _newDatabasePathOutput(""),
00146         _openedDatabasePath(""),
00147         _databaseUpdated(false),
00148         _odomImageShow(true),
00149         _odomImageDepthShow(false),
00150         _savedMaximized(false),
00151         _waypointsIndex(0),
00152         _cachedMemoryUsage(0),
00153         _createdCloudsMemoryUsage(0),
00154         _octomap(0),
00155         _odometryCorrection(Transform::getIdentity()),
00156         _processingOdometry(false),
00157         _oneSecondTimer(0),
00158         _elapsedTime(0),
00159         _posteriorCurve(0),
00160         _likelihoodCurve(0),
00161         _rawLikelihoodCurve(0),
00162         _autoScreenCaptureOdomSync(false),
00163         _autoScreenCaptureRAM(false),
00164         _firstCall(true)
00165 {
00166         UDEBUG("");
00167 
00168         initGuiResource();
00169 
00170         QPixmap pixmap(":images/RTAB-Map.png");
00171         QSplashScreen splash(pixmap);
00172         splash.show();
00173         splash.showMessage(tr("Loading..."));
00174         QApplication::processEvents();
00175 
00176         // Create dialogs
00177         _aboutDialog = new AboutDialog(this);
00178         _aboutDialog->setObjectName("AboutDialog");
00179         _exportCloudsDialog = new ExportCloudsDialog(this);
00180         _exportCloudsDialog->setObjectName("ExportCloudsDialog");
00181         _exportScansDialog = new ExportScansDialog(this);
00182         _exportScansDialog->setObjectName("ExportScansDialog");
00183         _postProcessingDialog = new PostProcessingDialog(this);
00184         _postProcessingDialog->setObjectName("PostProcessingDialog");
00185 
00186         _ui = new Ui_mainWindow();
00187         UDEBUG("Setup ui...");
00188         _ui->setupUi(this);
00189 
00190         // Add cloud viewers
00191         // Note that we add them here manually because there is a crash issue
00192         // when adding them in a DockWidget of the *.ui file. The cloud viewer is 
00193         // created in a widget which is not yet linked to main window when the CloudViewer constructor
00194         // is called (see order in generated ui file). VTK needs to get the top 
00195         // level window at the time CloudViewer is created, otherwise it may crash on some systems.
00196         _cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
00197         _cloudViewer->setObjectName("widget_cloudViewer");
00198         _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
00199         _loopClosureViewer = new LoopClosureViewer(_ui->layout_loopClosureViewer);
00200         _loopClosureViewer->setObjectName("widget_loopClosureViewer");
00201         _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
00202         UDEBUG("Setup ui... end");
00203 
00204         QString title("RTAB-Map[*]");
00205         this->setWindowTitle(title);
00206         this->setWindowIconText(tr("RTAB-Map"));
00207         this->setObjectName("MainWindow");
00208 
00209         //Setup dock widgets position if it is the first time the application is started.
00210         setDefaultViews();
00211 
00212         _ui->widget_mainWindow->setVisible(false);
00213 
00214         if(prefDialog)
00215         {
00216                 _preferencesDialog = prefDialog;
00217                 _preferencesDialog->setParent(this, Qt::Dialog);
00218         }
00219         else // Default dialog
00220         {
00221                 _preferencesDialog = new PreferencesDialog(this);
00222         }
00223 
00224         _preferencesDialog->setObjectName("PreferencesDialog");
00225         _preferencesDialog->init();
00226 
00227         // Restore window geometry
00228         bool statusBarShown = false;
00229         _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown);
00230         _preferencesDialog->loadWindowGeometry(_preferencesDialog);
00231         _preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
00232         _preferencesDialog->loadWindowGeometry(_exportScansDialog);
00233         _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
00234         _preferencesDialog->loadWindowGeometry(_aboutDialog);
00235         setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
00236 
00237 #ifdef RTABMAP_OCTOMAP
00238         _octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
00239 #endif
00240 
00241         // Timer
00242         _oneSecondTimer = new QTimer(this);
00243         _oneSecondTimer->setInterval(1000);
00244         _elapsedTime = new QTime();
00245         _ui->label_elapsedTime->setText("00:00:00");
00246         connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
00247         _logEventTime = new QTime();
00248         _logEventTime->start();
00249 
00250         //Graphics scenes
00251         _ui->imageView_source->setBackgroundColor(Qt::black);
00252         _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
00253         _ui->imageView_odometry->setBackgroundColor(Qt::black);
00254         _ui->imageView_odometry->setAlpha(200);
00255         _preferencesDialog->loadWidgetState(_ui->imageView_source);
00256         _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
00257         _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
00258         _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
00259 
00260         _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
00261         _ui->posteriorPlot->addCurve(_posteriorCurve, false);
00262         _ui->posteriorPlot->showLegend(false);
00263         _ui->posteriorPlot->setFixedYAxis(0,1);
00264         UPlotCurveThreshold * tc;
00265         tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
00266         connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
00267 
00268         _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00269         _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
00270         _ui->likelihoodPlot->showLegend(false);
00271 
00272         _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00273         _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
00274         _ui->rawLikelihoodPlot->showLegend(false);
00275 
00276         _initProgressDialog = new ProgressDialog(this);
00277         _initProgressDialog->setWindowTitle(tr("Progress dialog"));
00278         _initProgressDialog->setMinimumWidth(800);
00279 
00280         connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
00281 
00282         //connect stuff
00283         connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
00284         qRegisterMetaType<MainWindow::State>("MainWindow::State");
00285         connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
00286         connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
00287         qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
00288         connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
00289         qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
00290         connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
00291         connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
00292         connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
00293 
00294         // Dock Widget view actions (Menu->Window)
00295         _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
00296         _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
00297         _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
00298         _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
00299         _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
00300         _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
00301         _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
00302         _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
00303         _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
00304         _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
00305         _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
00306         _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
00307         _ui->toolBar->setWindowTitle(tr("File toolbar"));
00308         _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
00309         _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
00310         QAction * a = _ui->menuShow_view->addAction("Progress dialog");
00311         a->setCheckable(false);
00312         connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
00313         QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar");
00314         statusBarAction->setCheckable(true);
00315         statusBarAction->setChecked(statusBarShown);
00316         connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool)));
00317 
00318         // connect actions with custom slots
00319         connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
00320         connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
00321         connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00322         connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
00323         connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
00324         connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
00325         connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
00326         connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
00327         connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
00328         connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
00329         connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
00330         connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
00331         connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
00332         connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
00333         connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
00334         connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
00335         connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
00336         connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
00337         connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
00338         connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
00339         connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
00340         connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
00341         connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
00342         _ui->actionG2o_g2o->setVisible(Optimizer::isAvailable(Optimizer::kTypeG2O));
00343         connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
00344         connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
00345         connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
00346         connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
00347         connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
00348         connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
00349         connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
00350         connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
00351         connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
00352         connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
00353         connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
00354         connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
00355         connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
00356         connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
00357         connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
00358         connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
00359         connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
00360         connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom()));
00361         connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
00362         connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans()));
00363         connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
00364         connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages()));
00365         connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat()));
00366         connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans()));
00367         connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap()));
00368         connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
00369         connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
00370         connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
00371         connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
00372         connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
00373 
00374         _ui->actionPause->setShortcut(Qt::Key_Space);
00375         _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
00376         // Qt5 issue, we should explicitly add actions not in 
00377         // menu bar to have shortcut working
00378         this->addAction(_ui->actionSave_GUI_config);
00379         _ui->actionReset_Odometry->setEnabled(false);
00380         _ui->actionPost_processing->setEnabled(false);
00381         _ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
00382 
00383         QToolButton* toolButton = new QToolButton(this);
00384         toolButton->setMenu(_ui->menuSelect_source);
00385         toolButton->setPopupMode(QToolButton::InstantPopup);
00386         toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
00387         toolButton->setToolTip("Select sensor driver");
00388         _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
00389 
00390 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
00391         connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
00392 #else
00393         _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
00394 #endif
00395 
00396         //Settings menu
00397         connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
00398         connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
00399         connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00400         connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00401         connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
00402         connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
00403         connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
00404         connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00405         connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00406         connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00407         connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
00408         connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
00409         connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
00410         connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
00411         connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
00412         _ui->actionFreenect->setEnabled(CameraFreenect::available());
00413         _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
00414         _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
00415         _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
00416         _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
00417         _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
00418         _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
00419         _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
00420         _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
00421         _ui->actionStereoZed->setEnabled(CameraStereoZed::available());
00422         this->updateSelectSourceMenu();
00423 
00424         connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
00425 
00426         QActionGroup * modeGrp = new QActionGroup(this);
00427         modeGrp->addAction(_ui->actionSLAM_mode);
00428         modeGrp->addAction(_ui->actionLocalization_mode);
00429         _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
00430         _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
00431         connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00432         connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00433         connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
00434 
00435         // Settings changed
00436         qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
00437         connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
00438         qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
00439         connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
00440         // config GUI modified
00441         connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
00442         if(prefDialog == 0)
00443         {
00444                 connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
00445         }
00446         connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00447         connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00448         connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00449         connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00450         connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00451         connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00452         connect(_exportScansDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00453         connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00454         connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00455         connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
00456         connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00457         QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00458         for(int i=0; i<dockWidgets.size(); ++i)
00459         {
00460                 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
00461                 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00462         }
00463         connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00464         // catch resize events
00465         _ui->dockWidget_posterior->installEventFilter(this);
00466         _ui->dockWidget_likelihood->installEventFilter(this);
00467         _ui->dockWidget_rawlikelihood->installEventFilter(this);
00468         _ui->dockWidget_statsV2->installEventFilter(this);
00469         _ui->dockWidget_console->installEventFilter(this);
00470         _ui->dockWidget_loopClosureViewer->installEventFilter(this);
00471         _ui->dockWidget_mapVisibility->installEventFilter(this);
00472         _ui->dockWidget_graphViewer->installEventFilter(this);
00473         _ui->dockWidget_odometry->installEventFilter(this);
00474         _ui->dockWidget_cloudViewer->installEventFilter(this);
00475         _ui->dockWidget_imageView->installEventFilter(this);
00476 
00477         // more connects...
00478         _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
00479         _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
00480         _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
00481         connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
00482         connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
00483         connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
00484         connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
00485         connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
00486         connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
00487 
00488         // Statistics from the detector
00489         qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
00490         connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
00491 
00492         qRegisterMetaType<rtabmap::CameraInfo>("rtabmap::CameraInfo");
00493         connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo)));
00494 
00495         qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
00496         connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool)));
00497 
00498         connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
00499 
00500         // Apply state
00501         this->changeState(kIdle);
00502         this->applyPrefSettings(PreferencesDialog::kPanelAll);
00503 
00504         _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00505         _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00506         _cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00507         _cloudViewer->setBackfaceCulling(true, false);
00508         _preferencesDialog->loadWidgetState(_cloudViewer);
00509 
00510         //dialog states
00511         _preferencesDialog->loadWidgetState(_exportCloudsDialog);
00512         _preferencesDialog->loadWidgetState(_exportScansDialog);
00513         _preferencesDialog->loadWidgetState(_postProcessingDialog);
00514 
00515         if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
00516         {
00517                 const std::map<std::string, float> & statistics = Statistics::defaultData();
00518                 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
00519                 {
00520                         _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
00521                 }
00522         }
00523         // Specific MainWindow
00524         _ui->statsToolBox->updateStat("Planning/From/", 0.0f);
00525         _ui->statsToolBox->updateStat("Planning/Time/ms", 0.0f);
00526         _ui->statsToolBox->updateStat("Planning/Goal/", 0.0f);
00527         _ui->statsToolBox->updateStat("Planning/Poses/", 0.0f);
00528         _ui->statsToolBox->updateStat("Planning/Length/m", 0.0f);
00529 
00530         _ui->statsToolBox->updateStat("Camera/Time capturing/ms", 0.0f);
00531         _ui->statsToolBox->updateStat("Camera/Time decimation/ms", 0.0f);
00532         _ui->statsToolBox->updateStat("Camera/Time disparity/ms", 0.0f);
00533         _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", 0.0f);
00534         _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", 0.0f);
00535 
00536         _ui->statsToolBox->updateStat("Odometry/ID/", 0.0f);
00537         _ui->statsToolBox->updateStat("Odometry/Features/", 0.0f);
00538         _ui->statsToolBox->updateStat("Odometry/Matches/", 0.0f);
00539         _ui->statsToolBox->updateStat("Odometry/Inliers/", 0.0f);
00540         _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", 0.0f);
00541         _ui->statsToolBox->updateStat("Odometry/StdDev/", 0.0f);
00542         _ui->statsToolBox->updateStat("Odometry/Variance/", 0.0f);
00543         _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", 0.0f);
00544         _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", 0.0f);
00545         _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", 0.0f);
00546         _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", 0.0f);
00547         _ui->statsToolBox->updateStat("Odometry/Interval/ms", 0.0f);
00548         _ui->statsToolBox->updateStat("Odometry/Speed/kph", 0.0f);
00549         _ui->statsToolBox->updateStat("Odometry/Distance/m", 0.0f);
00550         _ui->statsToolBox->updateStat("Odometry/Tx/m", 0.0f);
00551         _ui->statsToolBox->updateStat("Odometry/Ty/m", 0.0f);
00552         _ui->statsToolBox->updateStat("Odometry/Tz/m", 0.0f);
00553         _ui->statsToolBox->updateStat("Odometry/Troll/deg", 0.0f);
00554         _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", 0.0f);
00555         _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", 0.0f);
00556         _ui->statsToolBox->updateStat("Odometry/Px/m", 0.0f);
00557         _ui->statsToolBox->updateStat("Odometry/Py/m", 0.0f);
00558         _ui->statsToolBox->updateStat("Odometry/Pz/m", 0.0f);
00559         _ui->statsToolBox->updateStat("Odometry/Proll/deg", 0.0f);
00560         _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", 0.0f);
00561         _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", 0.0f);
00562 
00563         _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", 0.0f);
00564         _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", 0.0f);
00565         _ui->statsToolBox->updateStat("GUI/RGB-D closure_view/ms", 0.0f);
00566         _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", 0.0f);
00567         _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", 0.0f);
00568         _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", 0.0f);
00569 
00570         this->loadFigures();
00571         connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
00572 
00573         // update loop closure viewer parameters
00574         _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
00575         _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
00576 
00577         splash.close();
00578 
00579         this->setFocus();
00580 
00581         UDEBUG("");
00582 }
00583 
00584 MainWindow::~MainWindow()
00585 {
00586         UDEBUG("");
00587         this->stopDetection();
00588         delete _ui;
00589         delete _elapsedTime;
00590         UDEBUG("");
00591 }
00592 
00593 void MainWindow::setupMainLayout(bool vertical)
00594 {
00595         if(vertical)
00596         {
00597                 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
00598         }
00599         else if(!vertical)
00600         {
00601                 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
00602         }
00603 }
00604 
00605 void MainWindow::closeEvent(QCloseEvent* event)
00606 {
00607         // Try to close all children
00608         /*QList<QMainWindow *> windows = this->findChildren<QMainWindow *>();
00609         for(int i=0; i<windows.size(); i++) {
00610                 if(!windows[i]->close()) {
00611                         event->setAccepted(false);
00612                         return;
00613                 }
00614         }*/
00615         UDEBUG("");
00616         bool processStopped = true;
00617         if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
00618         {
00619                 this->stopDetection();
00620                 if(_state == kInitialized)
00621                 {
00622                         if(this->closeDatabase())
00623                         {
00624                                 this->changeState(kApplicationClosing);
00625                         }
00626                 }
00627                 if(_state != kIdle)
00628                 {
00629                         processStopped = false;
00630                 }
00631         }
00632 
00633         if(processStopped)
00634         {
00635                 //write settings before quit?
00636                 bool save = false;
00637                 if(this->isWindowModified())
00638                 {
00639                         QMessageBox::Button b=QMessageBox::question(this,
00640                                         tr("RTAB-Map"),
00641                                         tr("There are unsaved changed settings. Save them?"),
00642                                         QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
00643                         if(b == QMessageBox::Save)
00644                         {
00645                                 save = true;
00646                         }
00647                         else if(b != QMessageBox::Discard)
00648                         {
00649                                 event->ignore();
00650                                 return;
00651                         }
00652                 }
00653 
00654                 if(save)
00655                 {
00656                         saveConfigGUI();
00657                 }
00658 
00659                 _ui->statsToolBox->closeFigures();
00660 
00661                 _ui->dockWidget_imageView->close();
00662                 _ui->dockWidget_likelihood->close();
00663                 _ui->dockWidget_rawlikelihood->close();
00664                 _ui->dockWidget_posterior->close();
00665                 _ui->dockWidget_statsV2->close();
00666                 _ui->dockWidget_console->close();
00667                 _ui->dockWidget_cloudViewer->close();
00668                 _ui->dockWidget_loopClosureViewer->close();
00669                 _ui->dockWidget_mapVisibility->close();
00670                 _ui->dockWidget_graphViewer->close();
00671                 _ui->dockWidget_odometry->close();
00672 
00673                 if(_camera)
00674                 {
00675                         UERROR("Camera must be already deleted here!");
00676                         delete _camera;
00677                         _camera = 0;
00678                 }
00679                 if(_odomThread)
00680                 {
00681                         UERROR("OdomThread must be already deleted here!");
00682                         delete _odomThread;
00683                         _odomThread = 0;
00684                 }
00685                 event->accept();
00686         }
00687         else
00688         {
00689                 event->ignore();
00690         }
00691         UDEBUG("");
00692 }
00693 
00694 void MainWindow::handleEvent(UEvent* anEvent)
00695 {
00696         if(anEvent->getClassName().compare("RtabmapEvent") == 0)
00697         {
00698                 RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
00699                 Statistics stats = rtabmapEvent->getStats();
00700                 int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
00701                 int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
00702                 bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
00703                 float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
00704                 if((stats.loopClosureId() > 0 &&
00705                         _ui->actionPause_on_match->isChecked())
00706                    ||
00707                    (stats.loopClosureId() == 0 &&
00708                     highestHypothesisId > 0 &&
00709                     highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
00710                     _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
00711                     rejectedHyp)
00712                    ||
00713                    (proximityClosureId > 0 &&
00714                     _ui->actionPause_on_local_loop_detection->isChecked()))
00715                 {
00716                         if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
00717                         {
00718                                 if(_preferencesDialog->beepOnPause())
00719                                 {
00720                                         QMetaObject::invokeMethod(this, "beep");
00721                                 }
00722                                 this->pauseDetection();
00723                         }
00724                 }
00725 
00726                 if(!_processingDownloadedMap)
00727                 {
00728                         _processingStatistics = true;
00729                         emit statsReceived(stats);
00730                 }
00731         }
00732         else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
00733         {
00734                 RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
00735                 emit rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
00736         }
00737         else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
00738         {
00739                 RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
00740                 emit rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
00741         }
00742         else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
00743         {
00744                 RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
00745                 emit rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
00746         }
00747         else if(anEvent->getClassName().compare("RtabmapLabelErrorEvent") == 0)
00748         {
00749                 RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
00750                 emit rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
00751         }
00752         else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
00753         {
00754                 emit rtabmapGoalStatusEventReceived(anEvent->getCode());
00755         }
00756         else if(anEvent->getClassName().compare("CameraEvent") == 0)
00757         {
00758                 CameraEvent * cameraEvent = (CameraEvent*)anEvent;
00759                 if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
00760                 {
00761                         if(_preferencesDialog->beepOnPause())
00762                         {
00763                                 QMetaObject::invokeMethod(this, "beep");
00764                         }
00765                         emit noMoreImagesReceived();
00766                 }
00767                 else
00768                 {
00769                         emit cameraInfoReceived(cameraEvent->info());
00770                         if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
00771                         {
00772                                 if (!_processingOdometry && !_processingStatistics)
00773                                 {
00774                                         _processingOdometry = true; // if we receive too many odometry events!
00775                                         OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
00776                                         emit odometryReceived(tmp, false);
00777                                 }
00778                                 else
00779                                 {
00780                                         // we receive too many odometry events! just send without data
00781                                         SensorData data(cv::Mat(), cameraEvent->data().id(), cameraEvent->data().stamp());
00782                                         data.setCameraModels(cameraEvent->data().cameraModels());
00783                                         data.setStereoCameraModel(cameraEvent->data().stereoCameraModel());
00784                                         data.setGroundTruth(cameraEvent->data().groundTruth());
00785                                         OdometryEvent tmp(data, cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
00786                                         emit odometryReceived(tmp, true);
00787                                 }
00788                         }
00789                 }
00790         }
00791         else if(anEvent->getClassName().compare("OdometryEvent") == 0)
00792         {
00793                 OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
00794                 if(!_processingOdometry && !_processingStatistics)
00795                 {
00796                         _processingOdometry = true; // if we receive too many odometry events!
00797                         emit odometryReceived(*odomEvent, false);
00798                 }
00799                 else
00800                 {
00801                         // we receive too many odometry events! just send without data
00802                         SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
00803                         data.setCameraModels(odomEvent->data().cameraModels());
00804                         data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
00805                         data.setGroundTruth(odomEvent->data().groundTruth());
00806                         OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info().copyWithoutData());
00807                         emit odometryReceived(tmp, true);
00808                 }
00809         }
00810         else if(anEvent->getClassName().compare("ULogEvent") == 0)
00811         {
00812                 ULogEvent * logEvent = (ULogEvent*)anEvent;
00813                 if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
00814                 {
00815                         QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
00816                         // The timer prevents multiple calls to pauseDetection() before the state can be changed
00817                         if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
00818                         {
00819                                 _logEventTime->start();
00820                                 if(_preferencesDialog->beepOnPause())
00821                                 {
00822                                         QMetaObject::invokeMethod(this, "beep");
00823                                 }
00824                                 pauseDetection();
00825                         }
00826                 }
00827         }
00828 }
00829 
00830 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
00831 {
00832         if(_firstStamp == 0.0)
00833         {
00834                 _firstStamp = info.stamp;
00835         }
00836 
00837         _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f);
00838         _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f);
00839         _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f);
00840         _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f);
00841         _ui->statsToolBox->updateStat("Camera/Time scan_from_depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f);
00842 }
00843 
00844 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
00845 {
00846         if(_firstStamp == 0.0)
00847         {
00848                 _firstStamp = odom.data().stamp();
00849         }
00850 
00851         UDEBUG("");
00852         _processingOdometry = true;
00853         UTimer time;
00854         // Process Data
00855 
00856         // Set color code as tooltip
00857         if(_ui->imageView_odometry->toolTip().isEmpty())
00858         {
00859                 _ui->imageView_odometry->setToolTip(
00860                         "Background Color Code:\n"
00861                         "  Dark Red = Odometry Lost\n"
00862                         "  Dark Yellow = Low Inliers\n"
00863                         "Feature Color code:\n"
00864                         "  Green = Inliers\n"
00865                         "  Yellow = Not matched features from previous frame(s)\n"
00866                         "  Red = Outliers");
00867         }
00868 
00869         Transform pose = odom.pose();
00870         bool lost = false;
00871         bool lostStateChanged = false;
00872 
00873         if(pose.isNull())
00874         {
00875                 UDEBUG("odom lost"); // use last pose
00876                 lostStateChanged = _cloudViewer->getBackgroundColor() != Qt::darkRed;
00877                 _cloudViewer->setBackgroundColor(Qt::darkRed);
00878                 _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
00879 
00880                 pose = _lastOdomPose;
00881                 lost = true;
00882         }
00883         else if(odom.info().inliers>0 &&
00884                         _preferencesDialog->getOdomQualityWarnThr() &&
00885                         odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
00886         {
00887                 UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
00888                 lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
00889                 _cloudViewer->setBackgroundColor(Qt::darkYellow);
00890                 _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
00891         }
00892         else
00893         {
00894                 UDEBUG("odom ok");
00895                 lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
00896                 _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
00897                 _ui->imageView_odometry->setBackgroundColor(Qt::black);
00898         }
00899 
00900         if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
00901         {
00902                 _lastOdomPose = pose;
00903                 _odometryReceived = true;
00904         }
00905 
00906         if(_ui->dockWidget_cloudViewer->isVisible())
00907         {
00908                 bool cloudUpdated = false;
00909                 bool scanUpdated = false;
00910                 bool featuresUpdated = false;
00911                 if(!pose.isNull())
00912                 {
00913                         // 3d cloud
00914                         if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
00915                            odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
00916                            !odom.data().depthOrRightRaw().empty() &&
00917                            (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
00918                            _preferencesDialog->isCloudsShown(1))
00919                         {
00920 
00921                                 if(odom.data().imageRaw().cols % _preferencesDialog->getCloudDecimation(1) != 0 ||
00922                                    odom.data().imageRaw().rows % _preferencesDialog->getCloudDecimation(1) != 0)
00923                                 {
00924                                         UERROR("Decimation (%d) is not modulo of the image resolution (%dx%d)! The cloud cannot be "
00925                                                         "created. Go to Preferences->3D Rendering under \"Odom\" column to modify this parameter.",
00926                                                         _preferencesDialog->getCloudDecimation(1),
00927                                                         odom.data().imageRaw().cols,
00928                                                         odom.data().imageRaw().rows);
00929                                 }
00930                                 else
00931                                 {
00932 
00933                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00934                                         pcl::IndicesPtr indices(new std::vector<int>);
00935                                         cloud = util3d::cloudRGBFromSensorData(odom.data(),
00936                                                         _preferencesDialog->getCloudDecimation(1),
00937                                                         _preferencesDialog->getCloudMaxDepth(1),
00938                                                         _preferencesDialog->getCloudMinDepth(1),
00939                                                         indices.get(),
00940                                                         _preferencesDialog->getAllParameters());
00941                                         if(indices->size())
00942                                         {
00943                                                 cloud = util3d::transformPointCloud(cloud, pose);
00944 
00945                                                 if(_preferencesDialog->isCloudMeshing())
00946                                                 {
00947                                                         // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
00948                                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
00949                                                         output = util3d::extractIndices(cloud, indices, false, true);
00950 
00951                                                         // Fast organized mesh
00952                                                         Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
00953                                                         if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
00954                                                         {
00955                                                                 viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
00956                                                                 viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
00957                                                                 viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
00958                                                         }
00959                                                         else if(!odom.data().stereoCameraModel().localTransform().isNull())
00960                                                         {
00961                                                                 viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
00962                                                                 viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
00963                                                                 viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
00964                                                         }
00965                                                         std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
00966                                                                         output,
00967                                                                         _preferencesDialog->getCloudMeshingAngle(),
00968                                                                         _preferencesDialog->isCloudMeshingQuad(),
00969                                                                         _preferencesDialog->getCloudMeshingTriangleSize(),
00970                                                                         Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
00971                                                         if(polygons.size())
00972                                                         {
00973                                                                 if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
00974                                                                 {
00975                                                                         UERROR("Adding cloudOdom to viewer failed!");
00976                                                                 }
00977                                                         }
00978                                                 }
00979                                                 else
00980                                                 {
00981                                                         if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
00982                                                         {
00983                                                                 UERROR("Adding cloudOdom to viewer failed!");
00984                                                         }
00985                                                 }
00986                                                 _cloudViewer->setCloudVisibility("cloudOdom", true);
00987                                                 _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
00988                                                 _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
00989 
00990                                                 cloudUpdated = true;
00991                                         }
00992                                 }
00993                         }
00994 
00995                         if(_preferencesDialog->isScansShown(1))
00996                         {
00997                                 // scan local map
00998                                 if(!odom.info().localScanMap.empty())
00999                                 {
01000                                         pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
01001                                         cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
01002                                         if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
01003                                         {
01004                                                 UERROR("Adding scanMapOdom to viewer failed!");
01005                                         }
01006                                         else
01007                                         {
01008                                                 _cloudViewer->setCloudVisibility("scanMapOdom", true);
01009                                                 _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
01010                                                 _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
01011                                                 scanUpdated = true;
01012                                         }
01013                                 }
01014                                 // scan cloud
01015                                 if(!odom.data().laserScanRaw().empty())
01016                                 {
01017                                         cv::Mat scan = odom.data().laserScanRaw();
01018 
01019                                         if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
01020                                         {
01021                                                 scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(1));
01022                                         }
01023 
01024                                         pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
01025                                         cloud = util3d::laserScanToPointCloudNormal(scan, pose);
01026                                         if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
01027                                         {
01028                                                 cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
01029                                         }
01030 
01031                                         if(!_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
01032                                         {
01033                                                 UERROR("Adding scanOdom to viewer failed!");
01034                                         }
01035                                         else
01036                                         {
01037                                                 _cloudViewer->setCloudVisibility("scanOdom", true);
01038                                                 _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
01039                                                 _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
01040                                                 scanUpdated = true;
01041                                         }
01042                                 }
01043                         }
01044 
01045                         // 3d features
01046                         if(_preferencesDialog->isFeaturesShown(1))
01047                         {
01048                                 if(!odom.info().localMap.empty())
01049                                 {
01050                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01051                                         cloud->resize(odom.info().localMap.size());
01052                                         int i=0;
01053                                         for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
01054                                         {
01055                                                 (*cloud)[i].x = iter->second.x;
01056                                                 (*cloud)[i].y = iter->second.y;
01057                                                 (*cloud)[i].z = iter->second.z;
01058 
01059                                                 // green = inlier, yellow = outliers
01060                                                 bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
01061                                                 (*cloud)[i].r = inlier?0:255;
01062                                                 (*cloud)[i].g = 255;
01063                                                 (*cloud)[i++].b = 0;
01064                                         }
01065 
01066                                         _cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
01067                                         _cloudViewer->setCloudVisibility("featuresOdom", true);
01068                                         _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
01069 
01070                                         featuresUpdated = true;
01071                                 }
01072                         }
01073                 }
01074                 if(!dataIgnored)
01075                 {
01076                         if(!cloudUpdated && _cloudViewer->getAddedClouds().contains("cloudOdom"))
01077                         {
01078                                 _cloudViewer->setCloudVisibility("cloudOdom", false);
01079                         }
01080                         if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanOdom"))
01081                         {
01082                                 _cloudViewer->setCloudVisibility("scanOdom", false);
01083                         }
01084                         if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanMapOdom"))
01085                         {
01086                                 _cloudViewer->setCloudVisibility("scanMapOdom", false);
01087                         }
01088                         if(!featuresUpdated && _cloudViewer->getAddedClouds().contains("featuresOdom"))
01089                         {
01090                                 _cloudViewer->setCloudVisibility("featuresOdom", false);
01091                         }
01092                 }
01093         }
01094 
01095         if(!odom.pose().isNull())
01096         {
01097                 // update camera position
01098                 _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
01099 
01100                 if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
01101                 {
01102                         _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
01103                 }
01104                 else if(!odom.data().stereoCameraModel().localTransform().isNull())
01105                 {
01106                         _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
01107                 }
01108 
01109         }
01110         _cloudViewer->update();
01111 
01112         if(_ui->graphicsView_graphView->isVisible())
01113         {
01114                 if(!pose.isNull() && !odom.pose().isNull())
01115                 {
01116                         _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
01117                         _ui->graphicsView_graphView->update();
01118                 }
01119         }
01120 
01121         if(_ui->dockWidget_odometry->isVisible() &&
01122            !odom.data().imageRaw().empty())
01123         {
01124                 if(_ui->imageView_odometry->isFeaturesShown())
01125                 {
01126                         if(odom.info().type == 0)
01127                         {
01128                                 _ui->imageView_odometry->setFeatures(
01129                                                 odom.info().words,
01130                                                 odom.data().depthRaw(),
01131                                                 Qt::yellow);
01132                         }
01133                         else if(odom.info().type == 1)
01134                         {
01135                                 std::vector<cv::KeyPoint> kpts;
01136                                 cv::KeyPoint::convert(odom.info().refCorners, kpts);
01137                                 _ui->imageView_odometry->setFeatures(
01138                                                 kpts,
01139                                                 odom.data().depthRaw(),
01140                                                 Qt::red);
01141                         }
01142                 }
01143 
01144                 //detect if it is OdometryMono intitialization
01145                 bool monoInitialization = false;
01146                 if(_preferencesDialog->getOdomStrategy() == 2 && odom.info().type == 1)
01147                 {
01148                         monoInitialization = true;
01149                 }
01150 
01151                 _ui->imageView_odometry->clearLines();
01152                 if(lost && !monoInitialization)
01153                 {
01154                         if(lostStateChanged)
01155                         {
01156                                 // save state
01157                                 _odomImageShow = _ui->imageView_odometry->isImageShown();
01158                                 _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
01159                         }
01160                         _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
01161                         _ui->imageView_odometry->setImageShown(true);
01162                         _ui->imageView_odometry->setImageDepthShown(true);
01163                 }
01164                 else
01165                 {
01166                         if(lostStateChanged)
01167                         {
01168                                 // restore state
01169                                 _ui->imageView_odometry->setImageShown(_odomImageShow);
01170                                 _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
01171                         }
01172 
01173                         _ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
01174                         if(_ui->imageView_odometry->isImageDepthShown())
01175                         {
01176                                 _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
01177                         }
01178 
01179                         if(odom.info().type == 0)
01180                         {
01181                                 if(_ui->imageView_odometry->isFeaturesShown())
01182                                 {
01183                                         for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
01184                                         {
01185                                                 _ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
01186                                         }
01187                                         for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
01188                                         {
01189                                                 _ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
01190                                         }
01191                                 }
01192                         }
01193                         if(odom.info().type == 1 && odom.info().refCorners.size())
01194                         {
01195                                 if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
01196                                 {
01197                                         //draw lines
01198                                         UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
01199                                         std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
01200                                         for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
01201                                         {
01202                                                 if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
01203                                                 {
01204                                                         _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
01205                                                 }
01206                                                 if(_ui->imageView_odometry->isLinesShown())
01207                                                 {
01208                                                         _ui->imageView_odometry->addLine(
01209                                                                         odom.info().refCorners[i].x,
01210                                                                         odom.info().refCorners[i].y,
01211                                                                         odom.info().newCorners[i].x,
01212                                                                         odom.info().newCorners[i].y,
01213                                                                         inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
01214                                                 }
01215                                         }
01216                                 }
01217                         }
01218                 }
01219                 if(!odom.data().imageRaw().empty())
01220                 {
01221                         _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
01222                 }
01223 
01224                 _ui->imageView_odometry->update();
01225         }
01226 
01227         if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
01228         {
01229                 this->captureScreen(_autoScreenCaptureRAM);
01230         }
01231 
01232         //Process info
01233         _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers);
01234         _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio);
01235         _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches);
01236         _ui->statsToolBox->updateStat("Odometry/StdDev/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().variance));
01237         _ui->statsToolBox->updateStat("Odometry/Variance/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().variance);
01238         _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
01239         _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
01240         _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features);
01241         _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize);
01242         _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize);
01243         _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id());
01244 
01245         float x=0.0f,y,z, roll,pitch,yaw;
01246         if(!odom.info().transform.isNull())
01247         {
01248                 odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01249                 _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x);
01250                 _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y);
01251                 _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z);
01252                 _ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI);
01253                 _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI);
01254                 _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI);
01255         }
01256 
01257         if(!odom.info().transformFiltered.isNull())
01258         {
01259                 odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01260                 _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x);
01261                 _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y);
01262                 _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z);
01263                 _ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI);
01264                 _ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI);
01265                 _ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI);
01266         }
01267         if(odom.info().interval > 0)
01268         {
01269                 _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().interval*1000.f);
01270                 _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f);
01271         }
01272 
01273         if(!odom.info().transformGroundTruth.isNull())
01274         {
01275                 odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01276                 _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x);
01277                 _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y);
01278                 _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z);
01279                 _ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI);
01280                 _ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI);
01281                 _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI);
01282         }
01283 
01284         //cumulative pose
01285         if(!odom.pose().isNull())
01286         {
01287                 odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01288                 _ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x);
01289                 _ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y);
01290                 _ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z);
01291                 _ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI);
01292                 _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI);
01293                 _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI);
01294         }
01295         if(!odom.data().groundTruth().isNull())
01296         {
01297                 odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01298                 _ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x);
01299                 _ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y);
01300                 _ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z);
01301                 _ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI);
01302                 _ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI);
01303                 _ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI);
01304         }
01305 
01306         if(odom.info().distanceTravelled > 0)
01307         {
01308                 _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().distanceTravelled);
01309         }
01310 
01311         _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), time.elapsed()*1000.0);
01312         _processingOdometry = false;
01313 }
01314 
01315 void MainWindow::processStats(const rtabmap::Statistics & stat)
01316 {
01317         _processingStatistics = true;
01318         ULOGGER_DEBUG("");
01319         QTime time, totalTime;
01320         time.start();
01321         totalTime.start();
01322         //Affichage des stats et images
01323 
01324         if(_firstStamp == 0.0)
01325         {
01326                 _firstStamp = stat.stamp();
01327         }
01328 
01329         int refMapId = -1, loopMapId = -1;
01330         if(uContains(stat.getSignatures(), stat.refImageId()))
01331         {
01332                 refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
01333         }
01334         int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
01335         int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
01336         if(_cachedSignatures.contains(loopId))
01337         {
01338                 loopMapId = _cachedSignatures.value(loopId).mapId();
01339         }
01340 
01341         _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
01342 
01343         if(stat.extended())
01344         {
01345                 float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
01346                 if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
01347                 {
01348                         UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
01349                 }
01350 
01351                 UDEBUG("");
01352                 bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
01353 
01354                 bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
01355 
01356                 // update cache
01357                 Signature signature;
01358                 if(uContains(stat.getSignatures(), stat.refImageId()))
01359                 {
01360                         signature = stat.getSignatures().at(stat.refImageId());
01361                         signature.sensorData().uncompressData(); // make sure data are uncompressed
01362 
01363                         if(!smallMovement)
01364                         {
01365                                 // keep in cache only compressed data
01366                                 Signature signatureWithoutRawData = signature;
01367                                 signatureWithoutRawData.sensorData().setImageRaw(cv::Mat());
01368                                 signatureWithoutRawData.sensorData().setDepthOrRightRaw(cv::Mat());
01369                                 signatureWithoutRawData.sensorData().setUserDataRaw(cv::Mat());
01370                                 signatureWithoutRawData.sensorData().setLaserScanRaw(cv::Mat(), 0, 0);
01371                                 _cachedSignatures.insert(signature.id(), signatureWithoutRawData);
01372                                 _cachedMemoryUsage += signatureWithoutRawData.sensorData().getMemoryUsed();
01373                         }
01374                 }
01375 
01376                 // For intermediate empty nodes, keep latest image shown
01377                 if(!signature.sensorData().imageRaw().empty() || signature.getWords().size())
01378                 {
01379                         _ui->imageView_source->clear();
01380                         _ui->imageView_loopClosure->clear();
01381 
01382                         _ui->imageView_source->setBackgroundColor(Qt::black);
01383                         _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
01384 
01385                         _ui->label_matchId->clear();
01386                 }
01387 
01388                 int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
01389                 bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
01390                 int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
01391                 bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
01392                 _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
01393 
01394                 if(rehearsalMerged > 0)
01395                 {
01396                         _ui->imageView_source->setBackgroundColor(Qt::blue);
01397                 }
01398                 else if(proximityTimeDetections > 0)
01399                 {
01400                         _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
01401                 }
01402                 else if(scanMatchingSuccess)
01403                 {
01404                         _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
01405                 }
01406                 else if(rehearsedSimilarity)
01407                 {
01408                         _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
01409                 }
01410                 else if(smallMovement)
01411                 {
01412                         _ui->imageView_source->setBackgroundColor(Qt::gray);
01413                 }
01414                 // Set color code as tooltip
01415                 if(_ui->label_refId->toolTip().isEmpty())
01416                 {
01417                         _ui->label_refId->setToolTip(
01418                                 "Background Color Code:\n"
01419                                 "  Blue = Weight Update Merged\n"
01420                                 "  Dark Blue = Weight Update\n"
01421                                 "  Dark Yellow = Proximity Detection in Time\n"
01422                                 "  Dark Cyan = Neighbor Link Refined\n"
01423                                 "  Gray = Small Movement\n"
01424                                 "Feature Color code:\n"
01425                                 "  Green = New\n"
01426                                 "  Yellow = New but Not Unique\n"
01427                                 "  Red = In Vocabulary\n"
01428                         "  Blue = In Vocabulary and in Previous Signature\n"
01429                                 "  Pink = In Vocabulary and in Loop Closure Signature");
01430                 }
01431                 // Set color code as tooltip
01432                 if(_ui->label_matchId->toolTip().isEmpty())
01433                 {
01434                         _ui->label_matchId->setToolTip(
01435                                 "Background Color Code:\n"
01436                                 "  Green = Accepted Loop Closure Detection\n"
01437                                 "  Red = Rejected Loop Closure Detection\n"
01438                                 "  Yellow = Proximity Detection in Space\n"
01439                                 "Feature Color code:\n"
01440                                 "  Red = In Vocabulary\n"
01441                                 "  Pink = In Vocabulary and in Loop Closure Signature");
01442                 }
01443 
01444                 UDEBUG("time= %d ms", time.restart());
01445 
01446                 int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
01447                 float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
01448                 int matchId = 0;
01449                 Signature loopSignature;
01450                 int shownLoopId = 0;
01451                 if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
01452                 {
01453                         bool show = true;
01454                         if(stat.loopClosureId() > 0)
01455                         {
01456                                 _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
01457                                 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
01458                                 if(highestHypothesisIsSaved)
01459                                 {
01460                                         _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
01461                                 }
01462                                 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
01463                                 matchId = stat.loopClosureId();
01464                         }
01465                         else if(stat.proximityDetectionId())
01466                         {
01467                                 _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
01468                                 _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
01469                                 matchId = stat.proximityDetectionId();
01470                         }
01471                         else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
01472                         {
01473                                 show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
01474                                 if(show)
01475                                 {
01476                                         _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
01477                                         _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
01478                                         _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
01479                                 }
01480                         }
01481                         else
01482                         {
01483                                 show = _preferencesDialog->imageHighestHypShown();
01484                                 if(show)
01485                                 {
01486                                         _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
01487                                 }
01488                         }
01489 
01490                         if(show)
01491                         {
01492                                 shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
01493                                 QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
01494                                 if(iter != _cachedSignatures.end())
01495                                 {
01496                                         // uncompress after copy to avoid keeping uncompressed data in memory
01497                                         loopSignature = iter.value();
01498                                         loopSignature.sensorData().uncompressData();
01499                                 }
01500                         }
01501                 }
01502                 _refIds.push_back(stat.refImageId());
01503                 _loopClosureIds.push_back(matchId);
01504 
01505                 //update image views
01506                 {
01507                         UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
01508                         UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
01509                         UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
01510                         UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
01511                         qimageThread.start();
01512                         qdepthThread.start();
01513                         qimageLoopThread.start();
01514                         qdepthLoopThread.start();
01515                         qimageThread.join();
01516                         qdepthThread.join();
01517                         qimageLoopThread.join();
01518                         qdepthLoopThread.join();
01519                         QImage img = qimageThread.getQImage();
01520                         QImage lcImg = qimageLoopThread.getQImage();
01521                         QImage depth = qdepthThread.getQImage();
01522                         QImage lcDepth = qdepthLoopThread.getQImage();
01523                         UDEBUG("time= %d ms", time.restart());
01524 
01525                         if(!img.isNull())
01526                         {
01527                                 _ui->imageView_source->setImage(img);
01528                         }
01529                         if(!depth.isNull())
01530                         {
01531                                 _ui->imageView_source->setImageDepth(depth);
01532                         }
01533                         if(!lcImg.isNull())
01534                         {
01535                                 _ui->imageView_loopClosure->setImage(lcImg);
01536                         }
01537                         if(!lcDepth.isNull())
01538                         {
01539                                 _ui->imageView_loopClosure->setImageDepth(lcDepth);
01540                         }
01541                         if(_ui->imageView_loopClosure->sceneRect().isNull())
01542                         {
01543                                 _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
01544                         }
01545                 }
01546 
01547                 UDEBUG("time= %d ms", time.restart());
01548 
01549                 // do it after scaling
01550                 this->drawKeypoints(signature.getWords(), loopSignature.getWords());
01551 
01552                 UDEBUG("time= %d ms", time.restart());
01553 
01554                 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size());
01555                 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size());
01556 
01557                 // PDF AND LIKELIHOOD
01558                 if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
01559                 {
01560                         UDEBUG("");
01561                         _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
01562 
01563                         ULOGGER_DEBUG("");
01564                         //Adjust thresholds
01565                         float value;
01566                         value = float(_preferencesDialog->getLoopThr());
01567                         emit(loopClosureThrChanged(value));
01568                 }
01569                 if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
01570                 {
01571                         _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
01572                 }
01573                 if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
01574                 {
01575                         _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
01576                 }
01577 
01578                 // Update statistics tool box
01579                 const std::map<std::string, float> & statistics = stat.data();
01580                 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
01581                 {
01582                         //ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
01583                         _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second);
01584                 }
01585 
01586                 UDEBUG("time= %d ms", time.restart());
01587 
01588                 //======================
01589                 // RGB-D Mapping stuff
01590                 //======================
01591                 UTimer timerVis;
01592 
01593                 // update clouds
01594                 if(stat.poses().size())
01595                 {
01596                         // update pose only if odometry is not received
01597                         std::map<int, int> mapIds;
01598                         std::map<int, Transform> groundTruth;
01599                         std::map<int, std::string> labels;
01600                         for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
01601                         {
01602                                 mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
01603                                 if(!iter->second.getGroundTruthPose().isNull())
01604                                 {
01605                                         groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
01606                                 }
01607                                 if(!iter->second.getLabel().empty())
01608                                 {
01609                                         labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
01610                                 }
01611                         }
01612 
01613                         std::map<int, Transform> poses = stat.poses();
01614                         Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth);
01615                         UDEBUG("time= %d ms", time.restart());
01616 
01617                         if(!_odometryReceived && poses.size())
01618                         {
01619                                 _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
01620 
01621                                 Transform localTransform = Transform::getIdentity();
01622                                 std::map<int, Signature>::const_iterator iter = stat.getSignatures().find(poses.rbegin()->first);
01623                                 if(iter != stat.getSignatures().end())
01624                                 {
01625                                         if(iter->second.sensorData().cameraModels().size() && !iter->second.sensorData().cameraModels()[0].localTransform().isNull())
01626                                         {
01627                                                 _cloudViewer->updateCameraFrustums(poses.rbegin()->second, iter->second.sensorData().cameraModels());
01628                                         }
01629                                         else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull())
01630                                         {
01631                                                 _cloudViewer->updateCameraFrustum(poses.rbegin()->second, iter->second.sensorData().stereoCameraModel());
01632                                         }
01633                                 }
01634 
01635                                 if(_ui->graphicsView_graphView->isVisible())
01636                                 {
01637                                         _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
01638                                 }
01639                         }
01640 
01641                         updateMapCloud(
01642                                         poses,
01643                                         stat.constraints(),
01644                                         mapIds,
01645                                         labels,
01646                                         groundTruth);
01647 
01648                         _odometryReceived = false;
01649 
01650                         _odometryCorrection = groundTruthOffset * stat.mapCorrection();
01651 
01652                         UDEBUG("time= %d ms", time.restart());
01653                         _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(timerVis.elapsed()*1000.0f));
01654 
01655                         // loop closure view
01656                         if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0)  &&
01657                            !stat.loopClosureTransform().isNull() &&
01658                            !loopSignature.sensorData().imageRaw().empty())
01659                         {
01660                                 // the last loop closure data
01661                                 Transform loopClosureTransform = stat.loopClosureTransform();
01662                                 signature.setPose(loopClosureTransform);
01663                                 _loopClosureViewer->setData(loopSignature, signature);
01664                                 if(_ui->dockWidget_loopClosureViewer->isVisible())
01665                                 {
01666                                         UTimer loopTimer;
01667                                         _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
01668                                         UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
01669                                         _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
01670                                 }
01671 
01672                                 UDEBUG("time= %d ms", time.restart());
01673                         }
01674 
01675                         // ground truth live statistics
01676                         if(poses.size() && groundTruth.size())
01677                         {
01678                                 std::vector<float> translationalErrors(poses.size());
01679                                 std::vector<float> rotationalErrors(poses.size());
01680                                 int oi=0;
01681                                 float sumTranslationalErrors = 0.0f;
01682                                 float sumRotationalErrors = 0.0f;
01683                                 float sumSqrdTranslationalErrors = 0.0f;
01684                                 float sumSqrdRotationalErrors = 0.0f;
01685                                 float radToDegree = 180.0f / M_PI;
01686                                 float translational_min = 0.0f;
01687                                 float translational_max = 0.0f;
01688                                 float rotational_min = 0.0f;
01689                                 float rotational_max = 0.0f;
01690                                 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
01691                                 {
01692                                         std::map<int, Transform>::iterator jter = groundTruth.find(iter->first);
01693                                         if(jter!=groundTruth.end())
01694                                         {
01695                                                 Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
01696                                                 Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
01697                                                 double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
01698                                                 rotationalErrors[oi] = a*radToDegree;
01699                                                 translationalErrors[oi] = iter->second.getDistance(jter->second);
01700 
01701                                                 sumTranslationalErrors+=translationalErrors[oi];
01702                                                 sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
01703                                                 sumRotationalErrors+=rotationalErrors[oi];
01704                                                 sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
01705 
01706                                                 if(oi == 0)
01707                                                 {
01708                                                         translational_min = translational_max = translationalErrors[oi];
01709                                                         rotational_min = rotational_max = rotationalErrors[oi];
01710                                                 }
01711                                                 else
01712                                                 {
01713                                                         if(translationalErrors[oi] < translational_min)
01714                                                         {
01715                                                                 translational_min = translationalErrors[oi];
01716                                                         }
01717                                                         else if(translationalErrors[oi] > translational_max)
01718                                                         {
01719                                                                 translational_max = translationalErrors[oi];
01720                                                         }
01721 
01722                                                         if(rotationalErrors[oi] < rotational_min)
01723                                                         {
01724                                                                 rotational_min = rotationalErrors[oi];
01725                                                         }
01726                                                         else if(rotationalErrors[oi] > rotational_max)
01727                                                         {
01728                                                                 rotational_max = rotationalErrors[oi];
01729                                                         }
01730                                                 }
01731 
01732                                                 ++oi;
01733                                         }
01734                                 }
01735                                 translationalErrors.resize(oi);
01736                                 rotationalErrors.resize(oi);
01737                                 if(oi)
01738                                 {
01739                                         float total = float(oi);
01740                                         float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
01741                                         float translational_mean = sumTranslationalErrors/total;
01742                                         float translational_median = translationalErrors[oi/2];
01743                                         float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
01744 
01745                                         float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
01746                                         float rotational_mean = sumRotationalErrors/total;
01747                                         float rotational_median = rotationalErrors[oi/2];
01748                                         float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
01749 
01750                                         UINFO("translational_rmse=%f", translational_rmse);
01751                                         UINFO("translational_mean=%f", translational_mean);
01752                                         UINFO("translational_median=%f", translational_median);
01753                                         UINFO("translational_std=%f", translational_std);
01754                                         UINFO("translational_min=%f", translational_min);
01755                                         UINFO("translational_max=%f", translational_max);
01756 
01757                                         UINFO("rotational_rmse=%f", rotational_rmse);
01758                                         UINFO("rotational_mean=%f", rotational_mean);
01759                                         UINFO("rotational_median=%f", rotational_median);
01760                                         UINFO("rotational_std=%f", rotational_std);
01761                                         UINFO("rotational_min=%f", rotational_min);
01762                                         UINFO("rotational_max=%f", rotational_max);
01763 
01764                                         _ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_rmse);
01765                                         _ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_mean);
01766                                         _ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_median);
01767                                         _ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_std);
01768                                         _ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_min);
01769                                         _ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_max);
01770 
01771                                         _ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_rmse);
01772                                         _ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_mean);
01773                                         _ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_median);
01774                                         _ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_std);
01775                                         _ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_min);
01776                                         _ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_max);
01777                                 }
01778                         }
01779                         UDEBUG("time= %d ms", time.restart());
01780                 }
01781 
01782                 if( _ui->graphicsView_graphView->isVisible())
01783                 {
01784                         // update posterior on the graph view
01785                         if(_preferencesDialog->isPosteriorGraphView() &&
01786                            stat.posterior().size())
01787                         {
01788                                 _ui->graphicsView_graphView->updatePosterior(stat.posterior());
01789                         }
01790                         // update local path on the graph view
01791                         _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
01792                         if(stat.localPath().size() == 0)
01793                         {
01794                                 // clear the global path if set (goal reached)
01795                                 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
01796                         }
01797                         // update current goal id
01798                         if(stat.currentGoalId() > 0)
01799                         {
01800                                 _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
01801                         }
01802                 }
01803 
01804                 UDEBUG("");
01805         }
01806         else if(!stat.extended() && stat.loopClosureId()>0)
01807         {
01808                 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
01809                 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
01810         }
01811         else
01812         {
01813                 _ui->label_matchId->clear();
01814         }
01815         float elapsedTime = static_cast<float>(totalTime.elapsed());
01816         UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
01817         _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime);
01818         if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
01819         {
01820                 this->captureScreen(_autoScreenCaptureRAM);
01821         }
01822 
01823         if(!_preferencesDialog->isImagesKept())
01824         {
01825                 _cachedSignatures.clear();
01826                 _cachedMemoryUsage = 0;
01827         }
01828         _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024));
01829         _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024));
01830 
01831         if(_state != kMonitoring && _state != kDetecting)
01832         {
01833                 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
01834                 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
01835         }
01836 
01837         _processingStatistics = false;
01838 }
01839 
01840 void MainWindow::updateMapCloud(
01841                 const std::map<int, Transform> & posesIn,
01842                 const std::multimap<int, Link> & constraints,
01843                 const std::map<int, int> & mapIdsIn,
01844                 const std::map<int, std::string> & labels,
01845                 const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
01846                 bool verboseProgress)
01847 {
01848         UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
01849                         (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
01850         if(posesIn.size())
01851         {
01852                 _currentPosesMap = posesIn;
01853                 _currentLinksMap = constraints;
01854                 _currentMapIds = mapIdsIn;
01855                 _currentLabels = labels;
01856                 _currentGTPosesMap = groundTruths;
01857                 if(_state != kMonitoring && _state != kDetecting)
01858                 {
01859                         _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
01860                         _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
01861                 }
01862                 _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
01863         }
01864 
01865         // filter duplicated poses
01866         std::map<int, Transform> poses;
01867         std::map<int, int> mapIds;
01868         if(_preferencesDialog->isCloudFiltering() && posesIn.size())
01869         {
01870                 float radius = _preferencesDialog->getCloudFilteringRadius();
01871                 float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
01872                 poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
01873                 for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
01874                 {
01875                         std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
01876                         if(jter!=mapIdsIn.end())
01877                         {
01878                                 mapIds.insert(*jter);
01879                         }
01880                         else
01881                         {
01882                                 UERROR("map id of node %d not found!", iter->first);
01883                         }
01884                 }
01885 
01886                 if(verboseProgress)
01887                 {
01888                         _initProgressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(posesIn.size()));
01889                         QApplication::processEvents();
01890                 }
01891         }
01892         else
01893         {
01894                 poses = posesIn;
01895                 mapIds = mapIdsIn;
01896         }
01897 
01898         std::map<int, bool> posesMask;
01899         for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
01900         {
01901                 posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
01902         }
01903         _ui->widget_mapVisibility->setMap(posesIn, posesMask);
01904 
01905         if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
01906         {
01907                 for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
01908                 {
01909                         std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
01910                         if(gtIter!=_currentGTPosesMap.end())
01911                         {
01912                                 iter->second = gtIter->second;
01913                         }
01914                         else
01915                         {
01916                                 UWARN("Not found ground truth pose for node %d", iter->first);
01917                         }
01918                 }
01919         }
01920         else if(_currentGTPosesMap.size() == 0)
01921         {
01922                 _ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
01923         }
01924 
01925         // Map updated! regenerate the assembled cloud, last pose is the new one
01926         UDEBUG("Update map with %d locations", poses.size());
01927         QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
01928         int i=1;
01929         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
01930         {
01931                 if(!iter->second.isNull())
01932                 {
01933                         std::string cloudName = uFormat("cloud%d", iter->first);
01934 
01935                         // 3d point cloud
01936                         bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
01937                         bool updateProjMap =
01938                                         _ui->graphicsView_graphView->isVisible() &&
01939                                         _ui->graphicsView_graphView->isGridMapVisible() &&
01940                                         _preferencesDialog->isGridMapFrom3DCloud() &&
01941                                         _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end();
01942                         bool updateOctomap = false;
01943 #ifdef RTABMAP_OCTOMAP
01944                         updateOctomap =
01945                                         _cloudViewer->isVisible() &&
01946                                         _preferencesDialog->isOctomapShown() &&
01947                                         _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
01948 #endif
01949                         if(update3dCloud ||     updateProjMap || updateOctomap)
01950                         {
01951                                 // update cloud
01952                                 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
01953                                 if(viewerClouds.contains(cloudName))
01954                                 {
01955                                         // Update only if the pose has changed
01956                                         Transform tCloud;
01957                                         _cloudViewer->getPose(cloudName, tCloud);
01958                                         if(tCloud.isNull() || iter->second != tCloud)
01959                                         {
01960                                                 if(!_cloudViewer->updateCloudPose(cloudName, iter->second))
01961                                                 {
01962                                                         UERROR("Updating pose cloud %d failed!", iter->first);
01963                                                 }
01964                                         }
01965                                         _cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
01966                                         _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
01967                                         _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
01968                                 }
01969                                 else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first))
01970                                 {
01971                                         createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
01972                                         if(_cloudViewer->getAddedClouds().contains(cloudName))
01973                                         {
01974                                                 _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
01975                                         }
01976                                 }
01977 
01978                                 //Update projection map
01979                                 if(updateProjMap || updateOctomap)
01980                                 {
01981                                         std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first);
01982                                         if(cloudIter != _cachedClouds.end())
01983                                         {
01984                                                 createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second, updateOctomap);
01985                                         }
01986                                         else if(createdCloud.first.get() && createdCloud.first->size() && createdCloud.second->size())
01987                                         {
01988                                                 createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second, updateOctomap);
01989                                         }
01990                                 }
01991                         }
01992                         else if(viewerClouds.contains(cloudName))
01993                         {
01994                                 _cloudViewer->setCloudVisibility(cloudName.c_str(), false);
01995                         }
01996 
01997                         // 2d point cloud
01998                         std::string scanName = uFormat("scan%d", iter->first);
01999                         if((_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
02000                                 (_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
02001                         {
02002                                 if(viewerClouds.contains(scanName))
02003                                 {
02004                                         // Update only if the pose has changed
02005                                         Transform tScan;
02006                                         _cloudViewer->getPose(scanName, tScan);
02007                                         if(tScan.isNull() || iter->second != tScan)
02008                                         {
02009                                                 if(!_cloudViewer->updateCloudPose(scanName, iter->second))
02010                                                 {
02011                                                         UERROR("Updating pose scan %d failed!", iter->first);
02012                                                 }
02013                                         }
02014                                         _cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
02015                                         _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
02016                                         _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
02017                                 }
02018                                 else if(_cachedSignatures.contains(iter->first))
02019                                 {
02020                                         QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
02021                                         if(!jter->sensorData().laserScanCompressed().empty() || !jter->sensorData().laserScanRaw().empty())
02022                                         {
02023                                                 this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
02024                                         }
02025                                 }
02026                         }
02027                         else if(viewerClouds.contains(scanName))
02028                         {
02029                                 _cloudViewer->setCloudVisibility(scanName.c_str(), false);
02030                         }
02031 
02032                         // 3d features
02033                         std::string featuresName = uFormat("features%d", iter->first);
02034                         if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
02035                         {
02036                                 if(viewerClouds.contains(featuresName))
02037                                 {
02038                                         // Update only if the pose has changed
02039                                         Transform tFeatures;
02040                                         _cloudViewer->getPose(featuresName, tFeatures);
02041                                         if(tFeatures.isNull() || iter->second != tFeatures)
02042                                         {
02043                                                 if(!_cloudViewer->updateCloudPose(featuresName, iter->second))
02044                                                 {
02045                                                         UERROR("Updating pose features %d failed!", iter->first);
02046                                                 }
02047                                         }
02048                                         _cloudViewer->setCloudVisibility(featuresName, _preferencesDialog->isFeaturesShown(0));
02049                                         _cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
02050                                 }
02051                                 else if(_cachedSignatures.contains(iter->first))
02052                                 {
02053                                         QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
02054                                         if(!jter->getWords3().empty())
02055                                         {
02056                                                 this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
02057                                         }
02058                                 }
02059                         }
02060                         else if(viewerClouds.contains(featuresName))
02061                         {
02062                                 _cloudViewer->setCloudVisibility(featuresName.c_str(), false);
02063                         }
02064 
02065                         if(verboseProgress)
02066                         {
02067                                 _initProgressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
02068                                 _initProgressDialog->incrementStep();
02069                                 if(poses.size() < 200 || i % 100 == 0)
02070                                 {
02071                                         QApplication::processEvents();
02072                                 }
02073                         }
02074                 }
02075 
02076                 ++i;
02077         }
02078 
02079         // activate actions
02080         if(_state != kMonitoring && _state != kDetecting)
02081         {
02082                 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
02083                 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
02084                 _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
02085                 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
02086                 _ui->actionView_scans->setEnabled(!_createdScans.empty());
02087 #ifdef RTABMAP_OCTOMAP
02088                 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
02089 #else
02090                 _ui->actionExport_octomap->setEnabled(false);
02091 #endif
02092         }
02093 
02094         //remove not used clouds
02095         for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
02096         {
02097                 std::list<std::string> splitted = uSplitNumChar(iter.key());
02098                 if(splitted.size() == 2)
02099                 {
02100                         int id = std::atoi(splitted.back().c_str());
02101                         if(poses.find(id) == poses.end())
02102                         {
02103                                 if(_cloudViewer->getCloudVisibility(iter.key()))
02104                                 {
02105                                         UDEBUG("Hide %s", iter.key().c_str());
02106                                         _cloudViewer->setCloudVisibility(iter.key(), false);
02107                                 }
02108                         }
02109                 }
02110         }
02111 
02112         UDEBUG("");
02113 
02114         // update 3D graphes (show all poses)
02115         _cloudViewer->removeAllGraphs();
02116         _cloudViewer->removeCloud("graph_nodes");
02117         if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
02118         {
02119                 // Find all graphs
02120                 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
02121                 for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
02122                 {
02123                         int mapId = uValue(_currentMapIds, iter->first, -1);
02124 
02125                         //edges
02126                         std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
02127                         if(kter == graphs.end())
02128                         {
02129                                 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
02130                         }
02131                         pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
02132                         kter->second->push_back(pt);
02133                 }
02134 
02135                 //Ground truth graph?
02136                 for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
02137                 {
02138                         int mapId = -100;
02139                         //edges
02140                         std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
02141                         if(kter == graphs.end())
02142                         {
02143                                 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
02144                         }
02145                         pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
02146                         kter->second->push_back(pt);
02147                 }
02148 
02149                 // add graphs
02150                 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
02151                 {
02152                         QColor color = Qt::gray;
02153                         if(iter->first >= 0)
02154                         {
02155                                 color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
02156                         }
02157                         _cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
02158                 }
02159         }
02160 
02161         UDEBUG("labels.size()=%d", (int)labels.size());
02162 
02163         // Update labels
02164         _cloudViewer->removeAllTexts();
02165         if(_preferencesDialog->isLabelsShown() && labels.size())
02166         {
02167                 for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
02168                 {
02169                         if(posesIn.find(iter->first)!=posesIn.end())
02170                         {
02171                                 int mapId = uValue(mapIdsIn, iter->first, -1);
02172                                 QColor color = Qt::gray;
02173                                 if(mapId >= 0)
02174                                 {
02175                                         color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
02176                                 }
02177                                 _cloudViewer->addOrUpdateText(
02178                                                 std::string("label_") + uNumber2Str(iter->first),
02179                                                 iter->second,
02180                                                 _currentPosesMap.at(iter->first),
02181                                                 0.1,
02182                                                 color);
02183                         }
02184                 }
02185         }
02186 
02187         UDEBUG("");
02188 
02189         // Update occupancy grid map in 3D map view and graph view
02190         if(_ui->graphicsView_graphView->isVisible())
02191         {
02192                 _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
02193                 _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
02194         }
02195         cv::Mat map8U;
02196         if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) &&
02197                         ((_gridLocalMaps.size() && !_preferencesDialog->isGridMapFrom3DCloud()) ||
02198                          (_projectionLocalMaps.size() && _preferencesDialog->isGridMapFrom3DCloud())))
02199         {
02200                 float xMin, yMin;
02201                 float resolution = _preferencesDialog->getGridMapResolution();
02202                 cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
02203                                         poses,
02204                                         _preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
02205                                         resolution,
02206                                         xMin, yMin,
02207                                         0,
02208                                         _preferencesDialog->isGridMapEroded());
02209                 if(!map8S.empty())
02210                 {
02211                         //convert to gray scaled map
02212                         map8U = util3d::convertMap2Image8U(map8S);
02213 
02214                         if(_preferencesDialog->getGridMapShown())
02215                         {
02216                                 float opacity = _preferencesDialog->getGridMapOpacity();
02217                                 _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
02218                         }
02219                         if(_ui->graphicsView_graphView->isVisible())
02220                         {
02221                                 _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
02222                         }
02223                 }
02224         }
02225         _ui->graphicsView_graphView->update();
02226 
02227         UDEBUG("");
02228 
02229         if(!_preferencesDialog->getGridMapShown())
02230         {
02231                 UDEBUG("");
02232                 _cloudViewer->removeOccupancyGridMap();
02233         }
02234 
02235 #ifdef RTABMAP_OCTOMAP
02236         _cloudViewer->removeOctomap();
02237         if(_preferencesDialog->isOctomapShown())
02238         {
02239                 UDEBUG("");
02240                 UTimer time;
02241                 _octomap->update(poses);
02242                 _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
02243                 UINFO("Octomap update time = %fs", time.ticks());
02244         }
02245 #endif
02246 
02247         if(viewerClouds.contains("cloudOdom"))
02248         {
02249                 if(!_preferencesDialog->isCloudsShown(1))
02250                 {
02251                         UDEBUG("");
02252                         _cloudViewer->setCloudVisibility("cloudOdom", false);
02253                 }
02254                 else
02255                 {
02256                         UDEBUG("");
02257                         _cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
02258                         _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
02259                         _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
02260                 }
02261         }
02262         if(viewerClouds.contains("scanOdom"))
02263         {
02264                 if(!_preferencesDialog->isScansShown(1))
02265                 {
02266                         UDEBUG("");
02267                         _cloudViewer->setCloudVisibility("scanOdom", false);
02268                 }
02269                 else
02270                 {
02271                         UDEBUG("");
02272                         _cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
02273                         _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
02274                         _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
02275                 }
02276         }
02277         if(viewerClouds.contains("scanMapOdom"))
02278         {
02279                 if(!_preferencesDialog->isScansShown(1))
02280                 {
02281                         UDEBUG("");
02282                         _cloudViewer->setCloudVisibility("scanMapOdom", false);
02283                 }
02284                 else
02285                 {
02286                         UDEBUG("");
02287                         _cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
02288                         _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
02289                         _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
02290                 }
02291         }
02292         if(viewerClouds.contains("featuresOdom"))
02293         {
02294                 if(!_preferencesDialog->isFeaturesShown(1))
02295                 {
02296                         UDEBUG("");
02297                         _cloudViewer->setCloudVisibility("featuresOdom", false);
02298                 }
02299                 else
02300                 {
02301                         UDEBUG("");
02302                         _cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
02303                         _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
02304                 }
02305         }
02306 
02307         UDEBUG("");
02308         _cloudViewer->update();
02309         UDEBUG("");
02310 }
02311 
02312 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
02313 {
02314         UDEBUG("");
02315         UASSERT(!pose.isNull());
02316         std::string cloudName = uFormat("cloud%d", nodeId);
02317         std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
02318         if(_cloudViewer->getAddedClouds().contains(cloudName))
02319         {
02320                 UERROR("Cloud %d already added to map.", nodeId);
02321                 return outputPair;
02322         }
02323 
02324         QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
02325         if(iter == _cachedSignatures.end())
02326         {
02327                 UERROR("Node %d is not in the cache.", nodeId);
02328                 return outputPair;
02329         }
02330 
02331         UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
02332 
02333         if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
02334            (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
02335         {
02336                 cv::Mat image, depth;
02337                 SensorData data = iter->sensorData();
02338                 data.uncompressData(&image, &depth, 0);
02339 
02340                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
02341                 pcl::IndicesPtr indices(new std::vector<int>);
02342                 UASSERT(nodeId == data.id());
02343 
02344                 if(image.cols % _preferencesDialog->getCloudDecimation(0) != 0 ||
02345                    image.rows % _preferencesDialog->getCloudDecimation(0) != 0)
02346                 {
02347                         UERROR("Decimation (%d) is not modulo of the image resolution (%dx%d)! The cloud cannot be "
02348                                         "created. Go to Preferences->3D Rendering under \"Map\" column to modify this parameter.",
02349                                         _preferencesDialog->getCloudDecimation(0),
02350                                         image.cols,
02351                                         image.rows);
02352                         return outputPair;
02353                 }
02354 
02355                 // Create organized cloud
02356                 cloud = util3d::cloudRGBFromSensorData(data,
02357                                 _preferencesDialog->getCloudDecimation(0),
02358                                 _preferencesDialog->getCloudMaxDepth(0),
02359                                 _preferencesDialog->getCloudMinDepth(0),
02360                                 indices.get(),
02361                                 _preferencesDialog->getAllParameters());
02362 
02363                 // view point
02364                 Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
02365                 if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
02366                 {
02367                         viewPoint[0] = data.cameraModels()[0].localTransform().x();
02368                         viewPoint[1] = data.cameraModels()[0].localTransform().y();
02369                         viewPoint[2] = data.cameraModels()[0].localTransform().z();
02370                 }
02371                 else if(!data.stereoCameraModel().localTransform().isNull())
02372                 {
02373                         viewPoint[0] = data.stereoCameraModel().localTransform().x();
02374                         viewPoint[1] = data.stereoCameraModel().localTransform().y();
02375                         viewPoint[2] = data.stereoCameraModel().localTransform().z();
02376                 }
02377 
02378                 // filtering pipeline
02379                 if(indices->size() && _preferencesDialog->getMapVoxel() > 0.0)
02380                 {
02381                         cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getMapVoxel());
02382                         //generate indices for all points (they are all valid)
02383                         indices->resize(cloud->size());
02384                         for(unsigned int i=0; i<cloud->size(); ++i)
02385                         {
02386                                 indices->at(i) = i;
02387                         }
02388                 }
02389 
02390                 // Do radius filtering after voxel filtering ( a lot faster)
02391                 if(indices->size() &&
02392                    _preferencesDialog->getMapNoiseRadius() > 0.0 &&
02393                    _preferencesDialog->getMapNoiseMinNeighbors() > 0)
02394                 {
02395                         indices = rtabmap::util3d::radiusFiltering(
02396                                         cloud,
02397                                         indices,
02398                                         _preferencesDialog->getMapNoiseRadius(),
02399                                         _preferencesDialog->getMapNoiseMinNeighbors());
02400                 }
02401 
02402                 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02403                 if(_preferencesDialog->isSubtractFiltering() &&
02404                    _preferencesDialog->getSubtractFilteringRadius() > 0.0)
02405                 {
02406                         pcl::IndicesPtr beforeFiltering = indices;
02407                         if(     cloud->size() &&
02408                                 _previousCloud.first>0 &&
02409                                 _previousCloud.second.first.first.get() != 0 &&
02410                                 _previousCloud.second.second.get() != 0 &&
02411                                 _previousCloud.second.second->size() &&
02412                                 _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
02413                         {
02414                                 UTimer time;
02415 
02416                                 rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
02417 
02418                                 //UWARN("saved new.pcd and old.pcd");
02419                                 //pcl::io::savePCDFile("new.pcd", *cloud, *indices);
02420                                 //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
02421 
02422                                 if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
02423                                 {
02424                                         //normals required
02425                                         if(_preferencesDialog->getNormalKSearch() > 0)
02426                                         {
02427                                                 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
02428                                                 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
02429                                         }
02430                                         else
02431                                         {
02432                                                 UWARN("Cloud subtraction with angle filtering is activated but "
02433                                                           "cloud normal K search is 0. Subtraction is done with angle.");
02434                                         }
02435                                 }
02436 
02437                                 if(cloudWithNormals->size() &&
02438                                         _previousCloud.second.first.second.get() &&
02439                                         _previousCloud.second.first.second->size())
02440                                 {
02441                                         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t);
02442                                         indices = rtabmap::util3d::subtractFiltering(
02443                                                         cloudWithNormals,
02444                                                         indices,
02445                                                         previousCloud,
02446                                                         _previousCloud.second.second,
02447                                                         _preferencesDialog->getSubtractFilteringRadius(),
02448                                                         _preferencesDialog->getSubtractFilteringAngle(),
02449                                                         _preferencesDialog->getSubtractFilteringMinPts());
02450                                 }
02451                                 else
02452                                 {
02453                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t);
02454                                         indices = rtabmap::util3d::subtractFiltering(
02455                                                         cloud,
02456                                                         indices,
02457                                                         previousCloud,
02458                                                         _previousCloud.second.second,
02459                                                         _preferencesDialog->getSubtractFilteringRadius(),
02460                                                         _preferencesDialog->getSubtractFilteringMinPts());
02461                                 }
02462 
02463 
02464                                 UINFO("Time subtract filtering %d from %d -> %d (%fs)",
02465                                                 (int)_previousCloud.second.second->size(),
02466                                                 (int)beforeFiltering->size(),
02467                                                 (int)indices->size(),
02468                                                 time.ticks());
02469                         }
02470                         // keep all indices for next subtraction
02471                         _previousCloud.first = nodeId;
02472                         _previousCloud.second.first.first = cloud;
02473                         _previousCloud.second.first.second = cloudWithNormals;
02474                         _previousCloud.second.second = beforeFiltering;
02475                 }
02476 
02477                 if(indices->size())
02478                 {
02479                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
02480                         bool added = false;
02481                         if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
02482                         {
02483                                 // Fast organized mesh
02484                                 // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
02485                                 output = util3d::extractIndices(cloud, indices, false, true);
02486                                 std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
02487                                                 output,
02488                                                 _preferencesDialog->getCloudMeshingAngle(),
02489                                                 _preferencesDialog->isCloudMeshingQuad(),
02490                                                 _preferencesDialog->getCloudMeshingTriangleSize(),
02491                                                 viewPoint);
02492                                 if(polygons.size())
02493                                 {
02494                                         // remove unused vertices to save memory
02495                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
02496                                         std::vector<pcl::Vertices> outputPolygons;
02497                                         util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
02498                                         if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
02499                                         {
02500                                                 UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
02501                                         }
02502                                         else
02503                                         {
02504                                                 added = true;
02505                                         }
02506                                 }
02507                         }
02508                         else
02509                         {
02510                                 if(_preferencesDialog->isCloudMeshing())
02511                                 {
02512                                         UWARN("Online meshing is activated but the generated cloud is "
02513                                                   "dense (voxel filtering is used or multiple cameras are used). Disable "
02514                                                   "online meshing in Preferences->3D Rendering to hide this warning.");
02515                                 }
02516 
02517                                 if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
02518                                 {
02519                                         pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
02520                                         pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
02521                                 }
02522 
02523                                 QColor color = Qt::gray;
02524                                 if(mapId >= 0)
02525                                 {
02526                                         color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
02527                                 }
02528 
02529                                 output = util3d::extractIndices(cloud, indices, false, true);
02530 
02531                                 if(cloudWithNormals->size())
02532                                 {
02533                                         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
02534                                         outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false);
02535 
02536                                         if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color))
02537                                         {
02538                                                 UERROR("Adding cloud %d to viewer failed!", nodeId);
02539                                         }
02540                                         else
02541                                         {
02542                                                 added = true;
02543                                         }
02544                                 }
02545                                 else
02546                                 {
02547                                         if(!_cloudViewer->addCloud(cloudName, output, pose, color))
02548                                         {
02549                                                 UERROR("Adding cloud %d to viewer failed!", nodeId);
02550                                         }
02551                                         else
02552                                         {
02553                                                 added = true;
02554                                         }
02555                                 }
02556                         }
02557                         if(added)
02558                         {
02559                                 outputPair.first = output;
02560                                 outputPair.second = indices;
02561                                 if(_preferencesDialog->isCloudsKept())
02562                                 {
02563                                         _cachedClouds.insert(std::make_pair(nodeId, outputPair));
02564                                         _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
02565                                 }
02566                         }
02567                 }
02568                 _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
02569                 _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
02570         }
02571 
02572         UDEBUG("");
02573         return outputPair;
02574 }
02575 
02576 void MainWindow::createAndAddProjectionMap(
02577                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02578                 const pcl::IndicesPtr & indices,
02579                 int nodeId,
02580                 const Transform & pose,
02581                 bool updateOctomap)
02582 {
02583         UDEBUG("");
02584         UASSERT(!pose.isNull());
02585 
02586         if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end() && !updateOctomap)
02587         {
02588                 UERROR("Projection map %d already added.", nodeId);
02589                 return;
02590         }
02591 
02592         if(indices->size())
02593         {
02594                 UTimer timer;
02595                 cv::Mat ground, obstacles;
02596                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloud;
02597 
02598                 // voxelize to grid cell size
02599                 if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
02600                 {
02601                         voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
02602                 }
02603 
02604                 // add pose rotation without yaw
02605                 float roll, pitch, yaw;
02606                 pose.getEulerAngles(roll, pitch, yaw);
02607                 voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, _preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0));
02608 
02609                 if(_preferencesDialog->projMaxObstaclesHeight())
02610                 {
02611                         voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
02612                 }
02613 
02614                 pcl::IndicesPtr groundIndices, obstaclesIndices;
02615                 util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
02616                                 voxelCloud,
02617                                 groundIndices,
02618                                 obstaclesIndices,
02619                                 20,
02620                                 _preferencesDialog->projMaxGroundAngle(),
02621                                 _preferencesDialog->getGridMapResolution()*2.0f,
02622                                 _preferencesDialog->projMinClusterSize(),
02623                                 _preferencesDialog->projFlatObstaclesDetected(),
02624                                 _preferencesDialog->projMaxGroundHeight());
02625 
02626                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02627                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02628 
02629                 if(groundIndices->size())
02630                 {
02631                         pcl::copyPointCloud(*voxelCloud, *groundIndices, *groundCloud);
02632                 }
02633 
02634                 if(obstaclesIndices->size())
02635                 {
02636                         pcl::copyPointCloud(*voxelCloud, *obstaclesIndices, *obstaclesCloud);
02637                 }
02638 
02639                 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
02640                                 groundCloud,
02641                                 obstaclesCloud,
02642                                 ground,
02643                                 obstacles,
02644                                 _preferencesDialog->getGridMapResolution());
02645 
02646                 if(updateOctomap)
02647                 {
02648                         // Update octomap
02649 #ifdef RTABMAP_OCTOMAP
02650                         if(_octomap->addedNodes().empty() ||
02651                                 nodeId > _octomap->addedNodes().rbegin()->first)
02652                         {
02653                                 Transform tinv = Transform(0,0,_preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0).inverse();
02654                                 groundCloud = util3d::transformPointCloud(groundCloud, tinv);
02655                                 obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
02656 
02657                                 if(_preferencesDialog->isOctomapGroundAnObstacle())
02658                                 {
02659                                         *obstaclesCloud += *groundCloud;
02660                                         groundCloud->clear();
02661                                 }
02662                                 _octomap->addToCache(nodeId, groundCloud, obstaclesCloud);
02663                         }
02664 #endif
02665                 }
02666 
02667                 _projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
02668                 UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
02669         }
02670         UDEBUG("");
02671 }
02672 
02673 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
02674 {
02675         std::string scanName = uFormat("scan%d", nodeId);
02676         if(_cloudViewer->getAddedClouds().contains(scanName))
02677         {
02678                 UERROR("Scan %d already added to map.", nodeId);
02679                 return;
02680         }
02681 
02682         QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
02683         if(iter == _cachedSignatures.end())
02684         {
02685                 UERROR("Node %d is not in the cache.", nodeId);
02686                 return;
02687         }
02688 
02689         if(!iter->sensorData().laserScanCompressed().empty() || !iter->sensorData().laserScanRaw().empty())
02690         {
02691                 cv::Mat scan;
02692                 iter->sensorData().uncompressData(0, 0, &scan);
02693 
02694                 if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
02695                 {
02696                         scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0));
02697                 }
02698 
02699                 if(scan.channels() == 6)
02700                 {
02701                         pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
02702                         cloud = util3d::laserScanToPointCloudNormal(scan);
02703                         if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
02704                         {
02705                                 cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
02706                         }
02707                         QColor color = Qt::gray;
02708                         if(mapId >= 0)
02709                         {
02710                                 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
02711                         }
02712                         if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
02713                         {
02714                                 UERROR("Adding cloud %d to viewer failed!", nodeId);
02715                         }
02716                         else
02717                         {
02718                                 if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
02719                                 {
02720                                         //reconvert the voxelized cloud
02721                                         scan = util3d::laserScanFromPointCloud(*cloud);
02722                                 }
02723                                 _createdScans.insert(std::make_pair(nodeId, scan));
02724                         }
02725                 }
02726                 else
02727                 {
02728                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02729                         cloud = util3d::laserScanToPointCloud(scan);
02730                         if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
02731                         {
02732                                 cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
02733                         }
02734                         QColor color = Qt::gray;
02735                         if(mapId >= 0)
02736                         {
02737                                 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
02738                         }
02739                         if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
02740                         {
02741                                 UERROR("Adding cloud %d to viewer failed!", nodeId);
02742                         }
02743                         else
02744                         {
02745                                 if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
02746                                 {
02747                                         //reconvert the voxelized cloud
02748                                         if(scan.channels() == 2)
02749                                         {
02750                                                 scan = util3d::laserScan2dFromPointCloud(*cloud);
02751                                         }
02752                                         else
02753                                         {
02754                                                 scan = util3d::laserScanFromPointCloud(*cloud);
02755                                         }
02756                                 }
02757                                 _createdScans.insert(std::make_pair(nodeId, scan));
02758 
02759                                 if(scan.channels() == 2)
02760                                 {
02761                                         cv::Mat ground, obstacles;
02762                                         util3d::occupancy2DFromLaserScan(scan, ground, obstacles, _preferencesDialog->getGridMapResolution());
02763                                         _gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
02764                                 }
02765                         }
02766                 }
02767                 _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
02768                 _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
02769         }
02770 }
02771 
02772 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
02773 {
02774         UDEBUG("");
02775         UASSERT(!pose.isNull());
02776         std::string cloudName = uFormat("features%d", nodeId);
02777         if(_cloudViewer->getAddedClouds().contains(cloudName))
02778         {
02779                 UERROR("Features cloud %d already added to map.", nodeId);
02780                 return;
02781         }
02782 
02783         QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
02784         if(iter == _cachedSignatures.end())
02785         {
02786                 UERROR("Node %d is not in the cache.", nodeId);
02787                 return;
02788         }
02789 
02790         if(_createdFeatures.find(nodeId) != _createdFeatures.end())
02791         {
02792                 UDEBUG("Features cloud %d already created.");
02793                 return;
02794         }
02795 
02796         if(iter->getWords3().size())
02797         {
02798                 UINFO("Create cloud from 3D words");
02799                 QColor color = Qt::gray;
02800                 if(mapId >= 0)
02801                 {
02802                         color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
02803                 }
02804 
02805                 cv::Mat rgb;
02806                 if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
02807                 {
02808                         SensorData data = iter->sensorData();
02809                         data.uncompressData(&rgb, 0, 0);
02810                 }
02811 
02812                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02813                 cloud->resize(iter->getWords3().size());
02814                 int oi=0;
02815                 UASSERT(iter->getWords().size() == iter->getWords3().size());
02816                 std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
02817                 for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
02818                                 jter!=iter->getWords3().end(); ++jter, ++kter, ++oi)
02819                 {
02820                         (*cloud)[oi].x = jter->second.x;
02821                         (*cloud)[oi].y = jter->second.y;
02822                         (*cloud)[oi].z = jter->second.z;
02823                         int u = kter->second.pt.x+0.5;
02824                         int v = kter->second.pt.y+0.5;
02825                         if(!rgb.empty() &&
02826                                 uIsInBounds(u, 0, rgb.cols-1) &&
02827                                 uIsInBounds(v, 0, rgb.rows-1))
02828                         {
02829                                 if(rgb.channels() == 1)
02830                                 {
02831                                         (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
02832                                 }
02833                                 else
02834                                 {
02835                                         cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
02836                                         (*cloud)[oi].b = bgr.val[0];
02837                                         (*cloud)[oi].g = bgr.val[1];
02838                                         (*cloud)[oi].r = bgr.val[2];
02839                                 }
02840                         }
02841                         else
02842                         {
02843                                 (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
02844                         }
02845                 }
02846                 if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
02847                 {
02848                         UERROR("Adding features cloud %d to viewer failed!", nodeId);
02849                 }
02850                 else
02851                 {
02852                         _createdFeatures.insert(std::make_pair(nodeId, cloud));
02853                 }
02854         }
02855         else
02856         {
02857                 return;
02858         }
02859 
02860         _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
02861         UDEBUG("");
02862 }
02863 
02864 Transform MainWindow::alignPosesToGroundTruth(
02865                 std::map<int, Transform> & poses,
02866                 const std::map<int, Transform> & groundTruth)
02867 {
02868         Transform t = Transform::getIdentity();
02869         if(groundTruth.size() && poses.size())
02870         {
02871                 unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
02872                 pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
02873                 cloud1.resize(maxSize);
02874                 cloud2.resize(maxSize);
02875                 int oi = 0;
02876                 int idFirst = 0;
02877                 for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
02878                 {
02879                         std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
02880                         if(iter2!=poses.end())
02881                         {
02882                                 if(oi==0)
02883                                 {
02884                                         idFirst = iter->first;
02885                                 }
02886                                 cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
02887                                 cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
02888                         }
02889                 }
02890 
02891                 if(oi>5)
02892                 {
02893                         cloud1.resize(oi);
02894                         cloud2.resize(oi);
02895                 
02896                         t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
02897                 }
02898                 else if(idFirst)
02899                 {
02900                         t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
02901                 }
02902                 if(!t.isIdentity())
02903                 {
02904                         for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02905                         {
02906                                 iter->second = t * iter->second;
02907                         }
02908                 }
02909                 UDEBUG("t=%s", t.prettyPrint().c_str());
02910         }
02911         return t;
02912 }
02913 
02914 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
02915 {
02916         if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
02917         {
02918                 QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
02919                 if(_preferencesDialog->isCloudsShown(0))
02920                 {
02921                         std::string cloudName = uFormat("cloud%d", nodeId);
02922                         if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
02923                         {
02924                                 createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
02925                         }
02926                         else if(viewerClouds.contains(cloudName))
02927                         {
02928                                 if(visible)
02929                                 {
02930                                         //make sure the transformation was done
02931                                         _cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second);
02932                                 }
02933                                 _cloudViewer->setCloudVisibility(cloudName, visible);
02934                         }
02935                 }
02936 
02937                 if(_preferencesDialog->isScansShown(0))
02938                 {
02939                         std::string scanName = uFormat("scan%d", nodeId);
02940                         if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
02941                         {
02942                                 createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
02943                         }
02944                         else if(viewerClouds.contains(scanName))
02945                         {
02946                                 if(visible)
02947                                 {
02948                                         //make sure the transformation was done
02949                                         _cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second);
02950                                 }
02951                                 _cloudViewer->setCloudVisibility(scanName, visible);
02952                         }
02953                 }
02954         }
02955         _cloudViewer->update();
02956 }
02957 
02958 void MainWindow::updateGraphView()
02959 {
02960         if(_ui->dockWidget_graphViewer->isVisible())
02961         {
02962                 UDEBUG("Graph visible!");
02963                 if(_currentPosesMap.size())
02964                 {
02965                         this->updateMapCloud(
02966                                         std::map<int, Transform>(_currentPosesMap),
02967                                         std::multimap<int, Link>(_currentLinksMap),
02968                                         std::map<int, int>(_currentMapIds),
02969                                         std::map<int, std::string>(_currentLabels),
02970                                         std::map<int, Transform>(_currentGTPosesMap));
02971                 }
02972         }
02973 }
02974 
02975 void MainWindow::processRtabmapEventInit(int status, const QString & info)
02976 {
02977         if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
02978         {
02979                 _initProgressDialog->resetProgress();
02980                 _initProgressDialog->show();
02981                 this->changeState(MainWindow::kInitializing);
02982         }
02983         else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
02984         {
02985                 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
02986                 this->changeState(MainWindow::kInitialized);
02987 
02988                 if(!_openedDatabasePath.isEmpty())
02989                 {
02990                         this->downloadAllClouds();
02991                 }
02992         }
02993         else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
02994         {
02995                 _initProgressDialog->resetProgress();
02996                 _initProgressDialog->show();
02997                 if(_state!=kApplicationClosing)
02998                 {
02999                         this->changeState(MainWindow::kClosing);
03000                 }
03001         }
03002         else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
03003         {
03004                 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
03005 
03006                 if(_databaseUpdated)
03007                 {
03008                         if(!_newDatabasePath.isEmpty())
03009                         {
03010                                 if(!_newDatabasePathOutput.isEmpty())
03011                                 {
03012                                         bool removed = true;
03013                                         if(QFile::exists(_newDatabasePathOutput))
03014                                         {
03015                                                 removed = QFile::remove(_newDatabasePathOutput);
03016                                         }
03017                                         if(removed)
03018                                         {
03019                                                 if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
03020                                                 {
03021                                                         std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
03022                                                         UINFO(msg.c_str());
03023                                                         QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
03024                                                 }
03025                                                 else
03026                                                 {
03027                                                         std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".",
03028                                                                         _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
03029                                                         UERROR(msg.c_str());
03030                                                         QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
03031                                                 }
03032                                         }
03033                                         else
03034                                         {
03035                                                 std::string msg = uFormat("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
03036                                                                 _newDatabasePathOutput.toStdString().c_str(), _newDatabasePath.toStdString().c_str());
03037                                                 UERROR(msg.c_str());
03038                                                 QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
03039                                         }
03040                                 }
03041                                 else if(QFile::remove(_newDatabasePath))
03042                                 {
03043                                         UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
03044                                 }
03045                                 else
03046                                 {
03047                                         UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
03048                                 }
03049 
03050                         }
03051                         else if(!_openedDatabasePath.isEmpty())
03052                         {
03053                                 std::string msg = uFormat("Database \"%s\" updated.", _openedDatabasePath.toStdString().c_str());
03054                                 UINFO(msg.c_str());
03055                                 QMessageBox::information(this, tr("Database updated!"), QString(msg.c_str()));
03056                         }
03057                 }
03058                 else if(!_newDatabasePath.isEmpty())
03059                 {
03060                         // just remove temporary database;
03061                         if(QFile::remove(_newDatabasePath))
03062                         {
03063                                 UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
03064                         }
03065                         else
03066                         {
03067                                 UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
03068                         }
03069                 }
03070                 _openedDatabasePath.clear();
03071                 _newDatabasePath.clear();
03072                 _newDatabasePathOutput.clear();
03073                 bool applicationClosing = _state == kApplicationClosing;
03074                 this->changeState(MainWindow::kIdle);
03075                 if(applicationClosing)
03076                 {
03077                         this->close();
03078                 }
03079         }
03080         else
03081         {
03082                 _initProgressDialog->incrementStep();
03083                 QString msg(info);
03084                 if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
03085                 {
03086                         _openedDatabasePath.clear();
03087                         _newDatabasePath.clear();
03088                         _newDatabasePathOutput.clear();
03089                         _initProgressDialog->setAutoClose(false);
03090                         msg.prepend(tr("[ERROR] "));
03091                         _initProgressDialog->appendText(msg);
03092                         this->changeState(MainWindow::kIdle);
03093                 }
03094                 else
03095                 {
03096                         _initProgressDialog->appendText(msg);
03097                 }
03098         }
03099 }
03100 
03101 void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event)
03102 {
03103         _initProgressDialog->appendText("Downloading the map... done.");
03104         _initProgressDialog->incrementStep();
03105 
03106         if(event.getCode())
03107         {
03108                 UERROR("Map received with code error %d!", event.getCode());
03109                 _initProgressDialog->appendText(uFormat("[ERROR] Map received with code error %d!", event.getCode()).c_str());
03110                 _initProgressDialog->setAutoClose(false);
03111         }
03112         else
03113         {
03114 
03115                 _processingDownloadedMap = true;
03116                 UINFO("Received map!");
03117                 _initProgressDialog->appendText(tr(" poses = %1").arg(event.getPoses().size()));
03118                 _initProgressDialog->appendText(tr(" constraints = %1").arg(event.getConstraints().size()));
03119 
03120                 _initProgressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
03121                 _initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
03122                 QApplication::processEvents();
03123 
03124                 int addedSignatures = 0;
03125                 std::map<int, int> mapIds;
03126                 std::map<int, Transform> groundTruth;
03127                 std::map<int, std::string> labels;
03128                 for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
03129                         iter!=event.getSignatures().end();
03130                         ++iter)
03131                 {
03132                         mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
03133                         if(!iter->second.getGroundTruthPose().isNull())
03134                         {
03135                                 groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
03136                         }
03137                         if(!iter->second.getLabel().empty())
03138                         {
03139                                 labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
03140                         }
03141                         if(!_cachedSignatures.contains(iter->first) ||
03142                            (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty()))
03143                         {
03144                                 _cachedSignatures.insert(iter->first, iter->second);
03145                                 _cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
03146                                 ++addedSignatures;
03147                         }
03148                         _initProgressDialog->incrementStep();
03149                         QApplication::processEvents();
03150                 }
03151                 _initProgressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
03152                 _initProgressDialog->incrementStep();
03153                 QApplication::processEvents();
03154 
03155                 _initProgressDialog->appendText("Inserting data in the cache... done.");
03156 
03157                 if(event.getPoses().size())
03158                 {
03159                         _initProgressDialog->appendText("Updating the 3D map cloud...");
03160                         _initProgressDialog->incrementStep();
03161                         QApplication::processEvents();
03162                         std::map<int, Transform> poses = event.getPoses();
03163                         alignPosesToGroundTruth(poses, groundTruth);
03164                         this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
03165                         _initProgressDialog->appendText("Updating the 3D map cloud... done.");
03166                 }
03167                 else
03168                 {
03169                         _initProgressDialog->appendText("No poses received! The map cloud cannot be updated...");
03170                         UINFO("Map received is empty! Cannot update the map cloud...");
03171                 }
03172 
03173                 _initProgressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
03174 
03175                 if(!_preferencesDialog->isImagesKept())
03176                 {
03177                         _cachedSignatures.clear();
03178                         _cachedMemoryUsage = 0;
03179                 }
03180                 if(_state != kMonitoring && _state != kDetecting)
03181                 {
03182                         _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
03183                         _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
03184                 }
03185                 _processingDownloadedMap = false;
03186         }
03187         _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
03188 }
03189 
03190 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
03191 {
03192         if(!event.getPoses().empty())
03193         {
03194                 _ui->graphicsView_graphView->setGlobalPath(event.getPoses());
03195         }
03196 
03197         _ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0));
03198         _ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0));
03199         _ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()));
03200         _ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()));
03201         _ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())));
03202 
03203         if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
03204         {
03205                 // use MessageBox
03206                 if(event.getPoses().empty())
03207                 {
03208                         QMessageBox * warn = new QMessageBox(
03209                                         QMessageBox::Warning,
03210                                         tr("Setting goal failed!"),
03211                                         tr("Setting goal to location %1%2 failed. "
03212                                                 "Some reasons: \n"
03213                                                 "1) the robot is not yet localized in the map,\n"
03214                                                 "2) the location doesn't exist in the map,\n"
03215                                                 "3) the location is not linked to the global map or\n"
03216                                                 "4) the location is too near of the current location (goal already reached).")
03217                                                 .arg(event.getGoal())
03218                                                 .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):""),
03219                                         QMessageBox::Ok,
03220                                         this);
03221                         warn->setAttribute(Qt::WA_DeleteOnClose, true);
03222                         warn->show();
03223                 }
03224                 else
03225                 {
03226                         QMessageBox * info = new QMessageBox(
03227                                         QMessageBox::Information,
03228                                         tr("Goal detected!"),
03229                                         tr("Global path computed to %1%2 (%3 poses, %4 m).")
03230                                         .arg(event.getGoal())
03231                                         .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):"")
03232                                         .arg(event.getPoses().size())
03233                                         .arg(graph::computePathLength(event.getPoses())),
03234                                         QMessageBox::Ok,
03235                                         this);
03236                         info->setAttribute(Qt::WA_DeleteOnClose, true);
03237                         info->show();
03238                 }
03239         }
03240         else if(event.getPoses().empty() && _waypoints.size())
03241         {
03242                 // resend the same goal
03243                 uSleep(1000);
03244                 this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
03245         }
03246 }
03247 
03248 void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
03249 {
03250         QMessageBox * warn = new QMessageBox(
03251                         QMessageBox::Warning,
03252                         tr("Setting label failed!"),
03253                         tr("Setting label %1 to location %2 failed. "
03254                                 "Some reasons: \n"
03255                                 "1) the location doesn't exist in the map,\n"
03256                                 "2) the location has already a label.").arg(label).arg(id),
03257                         QMessageBox::Ok,
03258                         this);
03259         warn->setAttribute(Qt::WA_DeleteOnClose, true);
03260         warn->show();
03261 }
03262 
03263 void MainWindow::processRtabmapGoalStatusEvent(int status)
03264 {
03265         _ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
03266         if(_waypoints.size())
03267         {
03268                 this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
03269         }
03270 }
03271 
03272 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
03273 {
03274         ULOGGER_DEBUG("");
03275         if(flags & PreferencesDialog::kPanelSource)
03276         {
03277                 // Camera settings...
03278                 _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
03279                 this->updateSelectSourceMenu();
03280                 _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
03281 
03282                 if(_camera)
03283                 {
03284                         if(dynamic_cast<DBReader*>(_camera->camera()) != 0)
03285                         {
03286                                 _camera->setImageRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
03287                         }
03288                         else
03289                         {
03290                                 _camera->setImageRate(_preferencesDialog->getGeneralInputRate());
03291                         }
03292                 }
03293 
03294         }//This will update the statistics toolbox
03295 
03296         if(flags & PreferencesDialog::kPanelGeneral)
03297         {
03298                 UDEBUG("General settings changed...");
03299                 setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
03300                 if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
03301                 {
03302                         _ui->graphicsView_graphView->clearPosterior();
03303                 }
03304         }
03305 
03306         if(flags & PreferencesDialog::kPanelCloudRendering)
03307         {
03308                 UDEBUG("Cloud rendering settings changed...");
03309                 if(_currentPosesMap.size())
03310                 {
03311                         this->updateMapCloud(
03312                                         std::map<int, Transform>(_currentPosesMap),
03313                                         std::multimap<int, Link>(_currentLinksMap),
03314                                         std::map<int, int>(_currentMapIds),
03315                                         std::map<int, std::string>(_currentLabels),
03316                                         std::map<int, Transform>(_currentGTPosesMap));
03317                 }
03318         }
03319 
03320         if(flags & PreferencesDialog::kPanelLogging)
03321         {
03322                 UDEBUG("Logging settings changed...");
03323                 ULogger::setLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerLevel());
03324                 ULogger::setEventLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerEventLevel());
03325                 ULogger::setType((ULogger::Type)_preferencesDialog->getGeneralLoggerType(),
03326                                                  (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
03327                 ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
03328                 ULogger::setPrintThreadId(_preferencesDialog->getGeneralLoggerPrintThreadId());
03329         }
03330 }
03331 
03332 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
03333 {
03334         applyPrefSettings(parameters, true); //post parameters
03335 }
03336 
03337 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
03338 {
03339         ULOGGER_DEBUG("");
03340         if(parameters.size())
03341         {
03342                 for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
03343                 {
03344                         UDEBUG("Parameter changed: Key=%s Value=%s", iter->first.c_str(), iter->second.c_str());
03345                 }
03346 
03347                 rtabmap::ParametersMap parametersModified = parameters;
03348 
03349                 if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
03350                 {
03351                         _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
03352                         _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
03353                         _cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
03354                 }
03355 
03356                 if(_state != kIdle && parametersModified.size())
03357                 {
03358                         if(parametersModified.erase(Parameters::kRtabmapWorkingDirectory()) &&
03359                                 _state != kMonitoring &&
03360                                 _state != kMonitoringPaused)
03361                         {
03362                                 QMessageBox::information(this, tr("Working memory changed"),
03363                                                 tr("The working directory can't be changed while the "
03364                                                                 "detector is running (state=%1). This will be "
03365                                                                 "applied when the detector will stop.").arg(_state));
03366                         }
03367                         if(postParamEvent)
03368                         {
03369                                 this->post(new ParamEvent(parametersModified));
03370                         }
03371                 }
03372 
03373                 // update loop closure viewer parameters (Use Map parameters)
03374                 _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
03375                 _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
03376 
03377                 // update graph view parameters
03378                 if(uContains(parameters, Parameters::kRGBDLocalRadius()))
03379                 {
03380                         _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
03381                 }
03382         }
03383 
03384         //update ui
03385         _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
03386         _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
03387         _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
03388 
03389         float value;
03390         value = float(_preferencesDialog->getLoopThr());
03391         emit(loopClosureThrChanged(value));
03392 }
03393 
03394 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
03395 {
03396         UTimer timer;
03397 
03398         timer.start();
03399         ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
03400         for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
03401         {
03402                 int id = iter->first;
03403                 QColor color;
03404                 if(uContains(loopWords, id))
03405                 {
03406                         // PINK = FOUND IN LOOP SIGNATURE
03407                         color = Qt::magenta;
03408                 }
03409                 else if(_lastIds.contains(id))
03410                 {
03411                         // BLUE = FOUND IN LAST SIGNATURE
03412                         color = Qt::blue;
03413                 }
03414                 else if(id<=_lastId)
03415                 {
03416                         // RED = ALREADY EXISTS
03417                         color = Qt::red;
03418                 }
03419                 else if(refWords.count(id) > 1)
03420                 {
03421                         // YELLOW = NEW and multiple times
03422                         color = Qt::yellow;
03423                 }
03424                 else
03425                 {
03426                         // GREEN = NEW
03427                         color = Qt::green;
03428                 }
03429                 _ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
03430         }
03431         ULOGGER_DEBUG("source time = %f s", timer.ticks());
03432 
03433         timer.start();
03434         ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
03435         QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
03436         for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
03437         {
03438                 int id = iter->first;
03439                 QColor color;
03440                 if(uContains(refWords, id))
03441                 {
03442                         // PINK = FOUND IN LOOP SIGNATURE
03443                         color = Qt::magenta;
03444                         //To draw lines... get only unique correspondences
03445                         if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
03446                         {
03447                                 const cv::KeyPoint & a = refWords.find(id)->second;
03448                                 const cv::KeyPoint & b = iter->second;
03449                                 uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
03450                         }
03451                 }
03452                 else if(id<=_lastId)
03453                 {
03454                         // RED = ALREADY EXISTS
03455                         color = Qt::red;
03456                 }
03457                 else if(refWords.count(id) > 1)
03458                 {
03459                         // YELLOW = NEW and multiple times
03460                         color = Qt::yellow;
03461                 }
03462                 else
03463                 {
03464                         // GREEN = NEW
03465                         color = Qt::green;
03466                 }
03467                 _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
03468         }
03469 
03470         ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
03471 
03472         if(refWords.size()>0)
03473         {
03474                 if((*refWords.rbegin()).first > _lastId)
03475                 {
03476                         _lastId = (*refWords.rbegin()).first;
03477                 }
03478                 _lastIds = QSet<int>::fromList(QList<int>::fromStdList(uKeysList(refWords)));
03479         }
03480 
03481         // Draw lines between corresponding features...
03482         float scaleSource = _ui->imageView_source->viewScale();
03483         float scaleLoop = _ui->imageView_loopClosure->viewScale();
03484         UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
03485         // Delta in actual window pixels
03486         float sourceMarginX = (_ui->imageView_source->width()   - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
03487         float sourceMarginY = (_ui->imageView_source->height()  - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
03488         float loopMarginX   = (_ui->imageView_loopClosure->width()   - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
03489         float loopMarginY   = (_ui->imageView_loopClosure->height()  - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
03490 
03491         float deltaX = 0;
03492         float deltaY = 0;
03493 
03494         if(_preferencesDialog->isVerticalLayoutUsed())
03495         {
03496                 deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
03497         }
03498         else
03499         {
03500                 deltaX = _ui->imageView_source->width();
03501         }
03502 
03503         for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
03504                 iter!=uniqueCorrespondences.end();
03505                 ++iter)
03506         {
03507 
03508                 _ui->imageView_source->addLine(
03509                                 iter->first.x,
03510                                 iter->first.y,
03511                                 (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
03512                                 (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
03513                                 Qt::cyan);
03514 
03515                 _ui->imageView_loopClosure->addLine(
03516                                 (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
03517                                 (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
03518                                 iter->second.x,
03519                                 iter->second.y,
03520                                 Qt::cyan);
03521         }
03522         _ui->imageView_source->update();
03523         _ui->imageView_loopClosure->update();
03524 }
03525 
03526 void MainWindow::showEvent(QShowEvent* anEvent)
03527 {
03528         //if the config file doesn't exist, make the GUI obsolete
03529         this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
03530 }
03531 
03532 void MainWindow::moveEvent(QMoveEvent* anEvent)
03533 {
03534         if(this->isVisible())
03535         {
03536                 // HACK, there is a move event when the window is shown the first time.
03537                 if(!_firstCall)
03538                 {
03539                         this->configGUIModified();
03540                 }
03541                 _firstCall = false;
03542         }
03543 }
03544 
03545 void MainWindow::resizeEvent(QResizeEvent* anEvent)
03546 {
03547         if(this->isVisible())
03548         {
03549                 this->configGUIModified();
03550         }
03551 }
03552 
03553 bool MainWindow::eventFilter(QObject *obj, QEvent *event)
03554 {
03555         if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
03556         {
03557                 this->setWindowModified(true);
03558         }
03559     else if(event->type() == QEvent::FileOpen )
03560     {
03561         openDatabase(((QFileOpenEvent*)event)->file());
03562     }
03563         return QWidget::eventFilter(obj, event);
03564 }
03565 
03566 void MainWindow::updateSelectSourceMenu()
03567 {
03568         _ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice);
03569 
03570         _ui->actionMore_options->setChecked(
03571                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
03572                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
03573                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
03574                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
03575                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
03576                         _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages
03577         );
03578 
03579         _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
03580         _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
03581         _ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
03582         _ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
03583         _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
03584         _ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
03585         _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
03586         _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
03587         _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
03588         _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
03589         _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
03590         _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
03591         _ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
03592 }
03593 
03594 void MainWindow::changeImgRateSetting()
03595 {
03596         emit imgRateChanged(_ui->doubleSpinBox_stats_imgRate->value());
03597 }
03598 
03599 void MainWindow::changeDetectionRateSetting()
03600 {
03601         emit detectionRateChanged(_ui->doubleSpinBox_stats_detectionRate->value());
03602 }
03603 
03604 void MainWindow::changeTimeLimitSetting()
03605 {
03606         emit timeLimitChanged((float)_ui->doubleSpinBox_stats_timeLimit->value());
03607 }
03608 
03609 void MainWindow::changeMappingMode()
03610 {
03611         emit mappingModeChanged(_ui->actionSLAM_mode->isChecked());
03612 }
03613 
03614 QString MainWindow::captureScreen(bool cacheInRAM)
03615 {
03616         QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png");
03617         _ui->statusbar->clearMessage();
03618         QPixmap figure = QPixmap::grabWidget(this);
03619 
03620         QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
03621         QString msg;
03622         if(cacheInRAM)
03623         {
03624                 msg = tr("Screen captured \"%1\"").arg(name);
03625                 QByteArray bytes;
03626                 QBuffer buffer(&bytes);
03627                 buffer.open(QIODevice::WriteOnly);
03628                 figure.save(&buffer, "PNG");
03629                 _autoScreenCaptureCachedImages.insert(name, bytes);
03630         }
03631         else
03632         {
03633                 QDir dir;
03634                 if(!dir.exists(targetDir))
03635                 {
03636                         dir.mkdir(targetDir);
03637                 }
03638                 targetDir += QDir::separator();
03639                 targetDir += "Main_window";
03640                 if(!dir.exists(targetDir))
03641                 {
03642                         dir.mkdir(targetDir);
03643                 }
03644                 targetDir += QDir::separator();
03645 
03646                 figure.save(targetDir + name);
03647                 msg = tr("Screen captured \"%1\"").arg(targetDir + name);
03648         }
03649         _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
03650         _ui->widget_console->appendMsg(msg);
03651 
03652         return targetDir + name;
03653 }
03654 
03655 void MainWindow::beep()
03656 {
03657         QApplication::beep();
03658 }
03659 
03660 void MainWindow::configGUIModified()
03661 {
03662         this->setWindowModified(true);
03663 }
03664 
03665 //ACTIONS
03666 void MainWindow::saveConfigGUI()
03667 {
03668         _savedMaximized = this->isMaximized();
03669         _preferencesDialog->saveMainWindowState(this);
03670         _preferencesDialog->saveWindowGeometry(_preferencesDialog);
03671         _preferencesDialog->saveWindowGeometry(_aboutDialog);
03672         _preferencesDialog->saveWidgetState(_cloudViewer);
03673         _preferencesDialog->saveWidgetState(_ui->imageView_source);
03674         _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
03675         _preferencesDialog->saveWidgetState(_ui->imageView_odometry);
03676         _preferencesDialog->saveWidgetState(_exportCloudsDialog);
03677         _preferencesDialog->saveWidgetState(_exportScansDialog);
03678         _preferencesDialog->saveWidgetState(_postProcessingDialog);
03679         _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
03680         _preferencesDialog->saveSettings();
03681         this->saveFigures();
03682         this->setWindowModified(false);
03683 }
03684 
03685 void MainWindow::newDatabase()
03686 {
03687         if(_state != MainWindow::kIdle)
03688         {
03689                 UERROR("This method can be called only in IDLE state.");
03690                 return;
03691         }
03692         _openedDatabasePath.clear();
03693         _newDatabasePath.clear();
03694         _newDatabasePathOutput.clear();
03695         _databaseUpdated = false;
03696         ULOGGER_DEBUG("");
03697         this->clearTheCache();
03698         std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
03699         if(QFile::exists(databasePath.c_str()))
03700         {
03701                 int r = QMessageBox::question(this,
03702                                 tr("Creating temporary database"),
03703                                 tr("Cannot create a new database because the temporary database \"%1\" already exists. "
03704                                   "There may be another instance of RTAB-Map running with the same Working Directory or "
03705                                   "the last time RTAB-Map was not closed correctly. "
03706                                   "Do you want to continue (the database will be deleted to create the new one)?").arg(databasePath.c_str()),
03707                                   QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
03708 
03709                 if(r == QMessageBox::Yes)
03710                 {
03711                         if(QFile::remove(databasePath.c_str()))
03712                         {
03713                                 UINFO("Deleted temporary database \"%s\".", databasePath.c_str());
03714                         }
03715                         else
03716                         {
03717                                 UERROR("Temporary database \"%s\" could not be deleted!", databasePath.c_str());
03718                                 return;
03719                         }
03720                 }
03721                 else
03722                 {
03723                         return;
03724                 }
03725         }
03726         _newDatabasePath = databasePath.c_str();
03727         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
03728         applyPrefSettings(_preferencesDialog->getAllParameters(), false);
03729 }
03730 
03731 void MainWindow::openDatabase()
03732 {
03733         QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
03734         if(!path.isEmpty())
03735         {
03736                 this->openDatabase(path);
03737         }
03738 }
03739 
03740 void MainWindow::openDatabase(const QString & path)
03741 {
03742         if(_state != MainWindow::kIdle)
03743         {
03744                 UERROR("Database can only be opened in IDLE state.");
03745                 return;
03746         }
03747 
03748         std::string value = path.toStdString();
03749         if(UFile::exists(value) &&
03750            UFile::getExtension(value).compare("db") == 0)
03751         {
03752                 _openedDatabasePath.clear();
03753                 _newDatabasePath.clear();
03754                 _newDatabasePathOutput.clear();
03755                 _databaseUpdated = false;
03756 
03757                 this->clearTheCache();
03758                 _openedDatabasePath = path;
03759 
03760                 // look if there are saved parameters
03761                 DBDriver * driver = DBDriver::create();
03762                 if(driver->openConnection(value, false))
03763                 {
03764                         ParametersMap parameters = driver->getLastParameters();
03765                         driver->closeConnection(false);
03766                         delete driver;
03767 
03768                         if(parameters.size())
03769                         {
03770                                 ParametersMap currentParameters = _preferencesDialog->getAllParameters();
03771                                 ParametersMap differentParameters;
03772                                 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
03773                                 {
03774                                         ParametersMap::iterator jter = currentParameters.find(iter->first);
03775                                         if(jter!=currentParameters.end() &&
03776                                            iter->second.compare(jter->second) != 0 &&
03777                                            iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
03778                                         {
03779                                                 differentParameters.insert(*iter);
03780                                                 QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
03781                                                                                 .arg(iter->first.c_str())
03782                                                                                 .arg(iter->second.c_str())
03783                                                                                 .arg(jter->second.c_str());
03784                                                 _ui->widget_console->appendMsg(msg);
03785                                                 UWARN(msg.toStdString().c_str());
03786                                         }
03787                                 }
03788 
03789                                 if(differentParameters.size())
03790                                 {
03791                                         int r = QMessageBox::question(this,
03792                                                         tr("Update parameters..."),
03793                                                         tr("The database is using %1 different parameter(s) than "
03794                                                            "those currently set in Preferences. Do you want "
03795                                                            "to use database's parameters?").arg(differentParameters.size()),
03796                                                         QMessageBox::Yes | QMessageBox::No,
03797                                                         QMessageBox::Yes);
03798                                         if(r == QMessageBox::Yes)
03799                                         {
03800                                                 _preferencesDialog->updateParameters(differentParameters);
03801                                         }
03802                                 }
03803                         }
03804                 }
03805 
03806                 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, value, 0, _preferencesDialog->getAllParameters()));
03807                 applyPrefSettings(_preferencesDialog->getAllParameters(), false);
03808         }
03809         else
03810         {
03811                 UERROR("File \"%s\" not valid.", value.c_str());
03812         }
03813 }
03814 
03815 bool MainWindow::closeDatabase()
03816 {
03817         if(_state != MainWindow::kInitialized)
03818         {
03819                 UERROR("This method can be called only in INITIALIZED state.");
03820                 return false;
03821         }
03822 
03823         _newDatabasePathOutput.clear();
03824         if(!_newDatabasePath.isEmpty() && _databaseUpdated)
03825         {
03826                 QMessageBox::Button b = QMessageBox::question(this,
03827                                 tr("Save database"),
03828                                 tr("Save the new database?"),
03829                                 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard,
03830                                 QMessageBox::Save);
03831 
03832                 if(b == QMessageBox::Save)
03833                 {
03834                         // Temp database used, automatically backup with unique name (timestamp)
03835                         QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
03836                         QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
03837 
03838                         newPath = QFileDialog::getSaveFileName(this, tr("Save database"), newPath, tr("RTAB-Map database files (*.db)"));
03839                         if(newPath.isEmpty())
03840                         {
03841                                 return false;
03842                         }
03843 
03844                         if(QFileInfo(newPath).suffix() == "")
03845                         {
03846                                 newPath += ".db";
03847                         }
03848 
03849                         _newDatabasePathOutput = newPath;
03850                 }
03851                 else if(b != QMessageBox::Discard)
03852                 {
03853                         return false;
03854                 }
03855         }
03856 
03857         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_openedDatabasePath.isEmpty() || !_newDatabasePathOutput.isEmpty()));
03858         return true;
03859 }
03860 
03861 void MainWindow::editDatabase()
03862 {
03863         if(_state != MainWindow::kIdle)
03864         {
03865                 UERROR("This method can be called only in IDLE state.");
03866                 return;
03867         }
03868         QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
03869         if(!path.isEmpty())
03870         {
03871                 DatabaseViewer * viewer = new DatabaseViewer(_preferencesDialog->getTmpIniFilePath(), this);
03872                 viewer->setWindowModality(Qt::WindowModal);
03873                 viewer->setAttribute(Qt::WA_DeleteOnClose, true);
03874                 viewer->showCloseButton();
03875 
03876                 if(viewer->isSavedMaximized())
03877                 {
03878                         viewer->showMaximized();
03879                 }
03880                 else
03881                 {
03882                         viewer->show();
03883                 }
03884 
03885                 viewer->openDatabase(path);
03886         }
03887 }
03888 
03889 void MainWindow::startDetection()
03890 {
03891         UDEBUG("");
03892         ParametersMap parameters = _preferencesDialog->getAllParameters();
03893         // verify source with input rates
03894         if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
03895            _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
03896            _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
03897            _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
03898            _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
03899            _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
03900         {
03901                 float inputRate = _preferencesDialog->getGeneralInputRate();
03902                 float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
03903                 int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
03904                 if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) &&
03905                         (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed()))
03906                 {
03907                         int button = QMessageBox::question(this,
03908                                         tr("Incompatible frame rates!"),
03909                                         tr("\"Source/Input rate\" (%1 Hz) is equal to/higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the "
03910                                            "source input is a directory of images/video/database, some images may be "
03911                                            "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over "
03912                                            "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to "
03913                                            "start the detection anyway?").arg(inputRate).arg(detectionRate),
03914                                          QMessageBox::Yes | QMessageBox::No);
03915                         if(button == QMessageBox::No)
03916                         {
03917                                 return;
03918                         }
03919                 }
03920                 if(bufferingSize != 0 &&
03921                   (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed()))
03922                 {
03923                         int button = QMessageBox::question(this,
03924                                         tr("Some images may be skipped!"),
03925                                         tr("\"RTAB-Map/Images buffer size\" is not infinite (size=%1). As the "
03926                                            "source input is a directory of images/video/database, some images may be "
03927                                            "skipped by the detector if the \"Source/Input rate\" (which is %2 Hz) is higher than the "
03928                                            "rate at which RTAB-Map can process the images. You may want to set the "
03929                                            "\"RTAB-Map/Images buffer size\" to 0 (infinite) to guaranty that all "
03930                                            "images are processed. Would you want to start the detection "
03931                                            "anyway?").arg(bufferingSize).arg(inputRate),
03932                                          QMessageBox::Yes | QMessageBox::No);
03933                         if(button == QMessageBox::No)
03934                         {
03935                                 return;
03936                         }
03937                 }
03938         }
03939 
03940         UDEBUG("");
03941         emit stateChanged(kStartingDetection);
03942 
03943         if(_camera != 0)
03944         {
03945                 QMessageBox::warning(this,
03946                                                      tr("RTAB-Map"),
03947                                                      tr("A camera is running, stop it first."));
03948                 UWARN("_camera is not null... it must be stopped first");
03949                 emit stateChanged(kInitialized);
03950                 return;
03951         }
03952 
03953         // Adjust pre-requirements
03954         if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
03955         {
03956                 QMessageBox::warning(this,
03957                                  tr("RTAB-Map"),
03958                                  tr("No sources are selected. See Preferences->Source panel."));
03959                 UWARN("No sources are selected. See Preferences->Source panel.");
03960                 emit stateChanged(kInitialized);
03961                 return;
03962         }
03963 
03964 
03965         Camera * camera = _preferencesDialog->createCamera();
03966         if(!camera)
03967         {
03968                 emit stateChanged(kInitialized);
03969                 return;
03970         }
03971 
03972         _camera = new CameraThread(camera, parameters);
03973         _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
03974         _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
03975         _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
03976         _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
03977         _camera->setScanFromDepth(
03978                         _preferencesDialog->isSourceScanFromDepth(),
03979                         _preferencesDialog->getSourceScanFromDepthDecimation(),
03980                         _preferencesDialog->getSourceScanFromDepthMaxDepth(),
03981                         _preferencesDialog->getSourceScanVoxelSize(),
03982                         _preferencesDialog->getSourceScanNormalsK());
03983 
03984         //Create odometry thread if rgbd slam
03985         if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
03986         {
03987                 // Require calibrated camera
03988                 if(!camera->isCalibrated())
03989                 {
03990                         UWARN("Camera is not calibrated!");
03991                         emit stateChanged(kInitialized);
03992                         delete _camera;
03993                         _camera = 0;
03994 
03995                         int button = QMessageBox::question(this,
03996                                         tr("Camera is not calibrated!"),
03997                                         tr("RTAB-Map cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
03998                                          QMessageBox::Yes | QMessageBox::No);
03999                         if(button == QMessageBox::Yes)
04000                         {
04001                                 QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
04002                         }
04003                         return;
04004                 }
04005                 else
04006                 {
04007                         if(_odomThread)
04008                         {
04009                                 UERROR("OdomThread must be already deleted here?!");
04010                                 delete _odomThread;
04011                                 _odomThread = 0;
04012                         }
04013 
04014                         if(!camera->odomProvided())
04015                         {
04016                                 Odometry * odom = Odometry::create(parameters);
04017                                 _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
04018 
04019                                 UEventsManager::addHandler(_odomThread);
04020                                 UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
04021                                 UEventsManager::createPipe(_camera, this, "CameraEvent");
04022                                 _odomThread->start();
04023                         }
04024                 }
04025         }
04026 
04027         if(_dataRecorder && _camera)
04028         {
04029                 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
04030         }
04031 
04032         _lastOdomPose.setNull();
04033         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer)); // clean sensors buffer
04034         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap)); // Trigger a new map
04035 
04036         if(_odomThread)
04037         {
04038                 _ui->actionReset_Odometry->setEnabled(true);
04039         }
04040 
04041         if(!_preferencesDialog->isStatisticsPublished())
04042         {
04043                 QMessageBox::information(this,
04044                                 tr("Information"),
04045                                 tr("Note that publishing statistics is disabled, "
04046                                    "progress will not be shown in the GUI."));
04047         }
04048 
04049 #ifdef RTABMAP_OCTOMAP
04050         UASSERT(_octomap != 0);
04051         delete _octomap;
04052         _octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
04053 #endif
04054 
04055         emit stateChanged(kDetecting);
04056 }
04057 
04058 // Could not be in the main thread here! (see handleEvents())
04059 void MainWindow::pauseDetection()
04060 {
04061         if(_camera)
04062         {
04063                 if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
04064                 {
04065                         // On Ctrl-click, start the camera and pause it automatically
04066                         emit stateChanged(kPaused);
04067                         if(_preferencesDialog->getGeneralInputRate())
04068                         {
04069                                 QTimer::singleShot(1000.0/_preferencesDialog->getGeneralInputRate() + 10, this, SLOT(pauseDetection()));
04070                         }
04071                         else
04072                         {
04073                                 emit stateChanged(kPaused);
04074                         }
04075                 }
04076                 else
04077                 {
04078                         emit stateChanged(kPaused);
04079                 }
04080         }
04081         else if(_state == kMonitoring)
04082         {
04083                 UINFO("Sending pause event!");
04084                 emit stateChanged(kMonitoringPaused);
04085         }
04086         else if(_state == kMonitoringPaused)
04087         {
04088                 UINFO("Sending unpause event!");
04089                 emit stateChanged(kMonitoring);
04090         }
04091 }
04092 
04093 void MainWindow::stopDetection()
04094 {
04095         if(!_camera && !_odomThread)
04096         {
04097                 return;
04098         }
04099 
04100         if(_state == kDetecting &&
04101            (_camera && _camera->isRunning()) )
04102         {
04103                 QMessageBox::StandardButton button = QMessageBox::question(this, tr("Stopping process..."), tr("Are you sure you want to stop the process?"), QMessageBox::Yes|QMessageBox::No, QMessageBox::No);
04104 
04105                 if(button != QMessageBox::Yes)
04106                 {
04107                         return;
04108                 }
04109         }
04110 
04111         ULOGGER_DEBUG("");
04112         // kill the processes
04113         if(_camera)
04114         {
04115                 _camera->join(true);
04116         }
04117 
04118         if(_odomThread)
04119         {
04120                 _ui->actionReset_Odometry->setEnabled(false);
04121                 _odomThread->kill();
04122         }
04123 
04124         // delete the processes
04125         if(_camera)
04126         {
04127                 delete _camera;
04128                 _camera = 0;
04129         }
04130         if(_odomThread)
04131         {
04132                 delete _odomThread;
04133                 _odomThread = 0;
04134         }
04135 
04136         if(_dataRecorder)
04137         {
04138                 delete _dataRecorder;
04139                 _dataRecorder = 0;
04140         }
04141 
04142         emit stateChanged(kInitialized);
04143 }
04144 
04145 void MainWindow::notifyNoMoreImages()
04146 {
04147         QMessageBox::information(this,
04148                         tr("No more images..."),
04149                         tr("The camera has reached the end of the stream."));
04150 }
04151 
04152 void MainWindow::printLoopClosureIds()
04153 {
04154         _ui->dockWidget_console->show();
04155         QString msgRef;
04156         QString msgLoop;
04157         for(int i = 0; i<_refIds.size(); ++i)
04158         {
04159                 msgRef.append(QString::number(_refIds[i]));
04160                 msgLoop.append(QString::number(_loopClosureIds[i]));
04161                 if(i < _refIds.size() - 1)
04162                 {
04163                         msgRef.append(" ");
04164                         msgLoop.append(" ");
04165                 }
04166         }
04167         _ui->widget_console->appendMsg(QString("IDs = [%1];").arg(msgRef));
04168         _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
04169 }
04170 
04171 void MainWindow::generateGraphDOT()
04172 {
04173         if(_graphSavingFileName.isEmpty())
04174         {
04175                 _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
04176         }
04177 
04178         bool ok;
04179         int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
04180         if(ok)
04181         {
04182                 int margin = 0;
04183                 if(id > 0)
04184                 {
04185                         margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
04186                 }
04187 
04188                 if(ok)
04189                 {
04190                         QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
04191                         if(!path.isEmpty())
04192                         {
04193                                 _graphSavingFileName = path;
04194                                 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
04195 
04196                                 _ui->dockWidget_console->show();
04197                                 _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName));
04198                         }
04199                 }
04200         }
04201 }
04202 
04203 void MainWindow::exportPosesRaw()
04204 {
04205         exportPoses(0);
04206 }
04207 void MainWindow::exportPosesRGBDSLAM()
04208 {
04209         exportPoses(1);
04210 }
04211 void MainWindow::exportPosesKITTI()
04212 {
04213         exportPoses(2);
04214 }
04215 void MainWindow::exportPosesTORO()
04216 {
04217         exportPoses(3);
04218 }
04219 void MainWindow::exportPosesG2O()
04220 {
04221         exportPoses(4);
04222 }
04223 
04224 void MainWindow::exportPoses(int format)
04225 {
04226         if(_currentPosesMap.size())
04227         {
04228                 std::map<int, double> stamps;
04229                 if(format == 1)
04230                 {
04231                         for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
04232                         {
04233                                 if(_cachedSignatures.contains(iter->first))
04234                                 {
04235                                         stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
04236                                 }
04237                         }
04238                         if(stamps.size()!=_currentPosesMap.size())
04239                         {
04240                                 QMessageBox::warning(this, tr("Export poses..."), tr("RGB-D SLAM format: Poses (%1) and stamps (%2) have not the same size! Try again after updating the cache.")
04241                                                 .arg(_currentPosesMap.size()).arg(stamps.size()));
04242                                 return;
04243                         }
04244                 }
04245 
04246                 if(_exportPosesFileName[format].isEmpty())
04247                 {
04248                         _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
04249                 }
04250 
04251                 QString path = QFileDialog::getSaveFileName(
04252                                 this,
04253                                 tr("Save File"),
04254                                 _exportPosesFileName[format],
04255                                 format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
04256 
04257                 if(!path.isEmpty())
04258                 {
04259                         _exportPosesFileName[format] = path;
04260 
04261                         bool saved = graph::exportPoses(path.toStdString(), format, _currentPosesMap, _currentLinksMap, stamps);
04262 
04263                         if(saved)
04264                         {
04265                                 QMessageBox::information(this,
04266                                                 tr("Export poses..."),
04267                                                 tr("%1 saved to \"%2\".")
04268                                                 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
04269                                                 .arg(_exportPosesFileName[format]));
04270                         }
04271                         else
04272                         {
04273                                 QMessageBox::information(this,
04274                                                 tr("Export poses..."),
04275                                                 tr("Failed to save %1 to \"%2\"!")
04276                                                 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
04277                                                 .arg(_exportPosesFileName[format]));
04278                         }
04279                 }
04280         }
04281 }
04282 
04283 void MainWindow::postProcessing()
04284 {
04285         if(_cachedSignatures.size() == 0)
04286         {
04287                 QMessageBox::warning(this,
04288                                 tr("Post-processing..."),
04289                                 tr("Signatures must be cached in the GUI for post-processing. "
04290                                    "Check the option in Preferences->General Settings (GUI), then "
04291                                    "refresh the cache."));
04292                 return;
04293         }
04294         if(_postProcessingDialog->exec() != QDialog::Accepted)
04295         {
04296                 return;
04297         }
04298 
04299         bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
04300         bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
04301         bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
04302         double clusterRadius = _postProcessingDialog->clusterRadius();
04303         double clusterAngle = _postProcessingDialog->clusterAngle();
04304         int detectLoopClosureIterations = _postProcessingDialog->iterations();
04305         bool sba = _postProcessingDialog->isSBA();
04306         int sbaIterations = _postProcessingDialog->sbaIterations();
04307         double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
04308         double sbaVariance = _postProcessingDialog->sbaVariance();
04309         Optimizer::Type sbaType = _postProcessingDialog->sbaType();
04310 
04311         if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
04312         {
04313                 UWARN("No post-processing selection...");
04314                 return;
04315         }
04316 
04317         // First, verify that we have all data required in the GUI
04318         bool allDataAvailable = true;
04319         std::map<int, Transform> odomPoses;
04320         for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
04321                         iter!=_currentPosesMap.end() && allDataAvailable;
04322                         ++iter)
04323         {
04324                 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
04325                 if(jter != _cachedSignatures.end())
04326                 {
04327                         if(jter->getPose().isNull())
04328                         {
04329                                 UWARN("Odometry pose of %d is null.", iter->first);
04330                                 allDataAvailable = false;
04331                         }
04332                         else
04333                         {
04334                                 odomPoses.insert(*iter); // fill raw poses
04335                         }
04336                 }
04337                 else
04338                 {
04339                         UWARN("Node %d missing.", iter->first);
04340                         allDataAvailable = false;
04341                 }
04342         }
04343 
04344         if(!allDataAvailable)
04345         {
04346                 QMessageBox::warning(this, tr("Not all data available in the GUI..."),
04347                                 tr("Some data missing in the cache to respect the constraints chosen. "
04348                                    "Try \"Edit->Download all clouds\" to update the cache and try again."));
04349                 return;
04350         }
04351 
04352         _initProgressDialog->resetProgress();
04353         _initProgressDialog->clear();
04354         _initProgressDialog->show();
04355         _initProgressDialog->appendText("Post-processing beginning!");
04356 
04357         int totalSteps = 0;
04358         if(refineNeighborLinks)
04359         {
04360                 totalSteps+=(int)odomPoses.size();
04361         }
04362         if(refineLoopClosureLinks)
04363         {
04364                 totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
04365         }
04366         if(sba)
04367         {
04368                 totalSteps+=1;
04369         }
04370         _initProgressDialog->setMaximumSteps(totalSteps);
04371         _initProgressDialog->show();
04372 
04373         ParametersMap parameters = _preferencesDialog->getAllParameters();
04374         Optimizer * optimizer = Optimizer::create(parameters);
04375         bool optimizeFromGraphEnd =  Parameters::defaultRGBDOptimizeFromGraphEnd();
04376         Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
04377 
04378         bool warn = false;
04379         int loopClosuresAdded = 0;
04380         std::multimap<int, int> checkedLoopClosures;
04381         if(detectMoreLoopClosures)
04382         {
04383                 UDEBUG("");
04384 
04385                 UASSERT(detectLoopClosureIterations>0);
04386                 for(int n=0; n<detectLoopClosureIterations; ++n)
04387                 {
04388                         _initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
04389                                         .arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
04390 
04391                         std::multimap<int, int> clusters = graph::radiusPosesClustering(
04392                                         _currentPosesMap,
04393                                         clusterRadius,
04394                                         clusterAngle*CV_PI/180.0);
04395 
04396                         _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+(int)clusters.size());
04397                         _initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
04398 
04399                         int i=0;
04400                         std::set<int> addedLinks;
04401                         for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
04402                         {
04403                                 int from = iter->first;
04404                                 int to = iter->second;
04405                                 if(iter->first < iter->second)
04406                                 {
04407                                         from = iter->second;
04408                                         to = iter->first;
04409                                 }
04410 
04411                                 bool alreadyChecked = false;
04412                                 for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
04413                                         !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
04414                                         ++jter)
04415                                 {
04416                                         if(to == jter->second)
04417                                         {
04418                                                 alreadyChecked = true;
04419                                         }
04420                                 }
04421 
04422                                 if(!alreadyChecked)
04423                                 {
04424                                         // only add new links and one per cluster per iteration
04425                                         if(addedLinks.find(from) == addedLinks.end() &&
04426                                            rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
04427                                         {
04428                                                 checkedLoopClosures.insert(std::make_pair(from, to));
04429 
04430                                                 if(!_cachedSignatures.contains(from))
04431                                                 {
04432                                                         UERROR("Didn't find signature %d", from);
04433                                                 }
04434                                                 else if(!_cachedSignatures.contains(to))
04435                                                 {
04436                                                         UERROR("Didn't find signature %d", to);
04437                                                 }
04438                                                 else
04439                                                 {
04440                                                         _initProgressDialog->incrementStep();
04441                                                         QApplication::processEvents();
04442 
04443                                                         Signature signatureFrom = _cachedSignatures[from];
04444                                                         Signature signatureTo = _cachedSignatures[to];
04445 
04446                                                         if(signatureFrom.getWeight() >= 0 &&
04447                                                            signatureTo.getWeight() >= 0) // ignore intermediate nodes
04448                                                         {
04449                                                                 Transform transform;
04450                                                                 RegistrationInfo info;
04451                                                                 RegistrationVis registration(parameters);
04452                                                                 transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
04453 
04454                                                                 if(!transform.isNull())
04455                                                                 {
04456                                                                         UINFO("Added new loop closure between %d and %d.", from, to);
04457                                                                         addedLinks.insert(from);
04458                                                                         addedLinks.insert(to);
04459                                                                         _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
04460                                                                         ++loopClosuresAdded;
04461                                                                         _initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
04462                                                                 }
04463                                                         }
04464                                                 }
04465                                         }
04466                                 }
04467                         }
04468                         _initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
04469                                         .arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
04470                         if(addedLinks.size() == 0)
04471                         {
04472                                 break;
04473                         }
04474 
04475                         if(n+1 < detectLoopClosureIterations)
04476                         {
04477                                 _initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
04478                                                 .arg(odomPoses.size()).arg(_currentLinksMap.size()));
04479                                 int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
04480                                 std::map<int, rtabmap::Transform> posesOut;
04481                                 std::multimap<int, rtabmap::Link> linksOut;
04482                                 std::map<int, rtabmap::Transform> optimizedPoses;
04483                                 optimizer->getConnectedGraph(
04484                                                 fromId,
04485                                                 odomPoses,
04486                                                 _currentLinksMap,
04487                                                 posesOut,
04488                                                 linksOut);
04489                                 optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
04490                                 _currentPosesMap = optimizedPoses;
04491                                 _initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
04492                         }
04493                 }
04494                 UINFO("Added %d loop closures.", loopClosuresAdded);
04495                 _initProgressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
04496         }
04497 
04498         if(refineNeighborLinks || refineLoopClosureLinks)
04499         {
04500                 UDEBUG("");
04501                 if(refineLoopClosureLinks)
04502                 {
04503                         _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
04504                 }
04505                 // TODO: support ICP from laser scans?
04506                 _initProgressDialog->appendText(tr("Refining links..."));
04507 
04508                 RegistrationIcp regIcp(parameters);
04509 
04510                 int i=0;
04511                 for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
04512                 {
04513                         int type = iter->second.type();
04514 
04515                         if((refineNeighborLinks && type==Link::kNeighbor) ||
04516                            (refineLoopClosureLinks && type!=Link::kNeighbor))
04517                         {
04518                                 int from = iter->second.from();
04519                                 int to = iter->second.to();
04520 
04521                                 _initProgressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
04522                                 _initProgressDialog->incrementStep();
04523                                 QApplication::processEvents();
04524 
04525                                 if(!_cachedSignatures.contains(from))
04526                                 {
04527                                         UERROR("Didn't find signature %d",from);
04528                                 }
04529                                 else if(!_cachedSignatures.contains(to))
04530                                 {
04531                                         UERROR("Didn't find signature %d", to);
04532                                 }
04533                                 else
04534                                 {
04535                                         Signature & signatureFrom = _cachedSignatures[from];
04536                                         Signature & signatureTo = _cachedSignatures[to];
04537 
04538                                         if(!signatureFrom.sensorData().laserScanRaw().empty() &&
04539                                            !signatureTo.sensorData().laserScanRaw().empty())
04540                                         {
04541                                                 RegistrationInfo info;
04542                                                 Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
04543 
04544                                                 if(!transform.isNull())
04545                                                 {
04546                                                         Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), info.variance, info.variance);
04547                                                         iter->second = newLink;
04548                                                 }
04549                                                 else
04550                                                 {
04551                                                         QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg.c_str());
04552                                                         _initProgressDialog->appendText(str, Qt::darkYellow);
04553                                                         UWARN("%s", str.toStdString().c_str());
04554                                                         warn = true;
04555                                                 }
04556                                         }
04557                                         else
04558                                         {
04559                                                 QString str;
04560                                                 if(signatureFrom.getWeight() < 0 || signatureTo.getWeight() < 0)
04561                                                 {
04562                                                         str = tr("Cannot refine link %1->%2 (Intermediate node detected!)").arg(from).arg(to);
04563                                                 }
04564                                                 else
04565                                                 {
04566                                                         str = tr("Cannot refine link %1->%2 (clouds empty!)").arg(from).arg(to);
04567                                                 }
04568 
04569                                                 _initProgressDialog->appendText(str, Qt::darkYellow);
04570                                                 UWARN("%s", str.toStdString().c_str());
04571                                                 warn = true;
04572                                         }
04573                                 }
04574                         }
04575                 }
04576                 _initProgressDialog->appendText(tr("Refining links...done!"));
04577         }
04578 
04579         _initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
04580                         .arg(odomPoses.size()).arg(_currentLinksMap.size()));
04581 
04582         int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
04583         std::map<int, rtabmap::Transform> posesOut;
04584         std::multimap<int, rtabmap::Link> linksOut;
04585         std::map<int, rtabmap::Transform> optimizedPoses;
04586         optimizer->getConnectedGraph(
04587                         fromId,
04588                         odomPoses,
04589                         _currentLinksMap,
04590                         posesOut,
04591                         linksOut);
04592         optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
04593         _initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
04594         _initProgressDialog->incrementStep();
04595 
04596         if(sba)
04597         {
04598                 UASSERT(Optimizer::isAvailable(sbaType));
04599                 _initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
04600                                         .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
04601                 QApplication::processEvents();
04602                 uSleep(100);
04603                 QApplication::processEvents();
04604 
04605                 ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
04606                 uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
04607                 uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
04608                 uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
04609                 Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
04610                 std::map<int, Transform>  newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
04611                 delete sba;
04612                 if(newPoses.size())
04613                 {
04614                         optimizedPoses = newPoses;
04615                         _initProgressDialog->appendText(tr("SBA... done!"));
04616                 }
04617                 else
04618                 {
04619                         _initProgressDialog->appendText(tr("SBA... failed!"));
04620                         _initProgressDialog->setAutoClose(false);
04621                 }
04622                 _initProgressDialog->incrementStep();
04623         }
04624 
04625         _initProgressDialog->appendText(tr("Updating map..."));
04626         alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
04627         this->updateMapCloud(
04628                         optimizedPoses,
04629                         std::multimap<int, Link>(_currentLinksMap),
04630                         std::map<int, int>(_currentMapIds),
04631                         std::map<int, std::string>(_currentLabels),
04632                         std::map<int, Transform>(_currentGTPosesMap),
04633                         false);
04634         _initProgressDialog->appendText(tr("Updating map... done!"));
04635 
04636         if(warn)
04637         {
04638                 _initProgressDialog->setAutoClose(false);
04639         }
04640 
04641         _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
04642         _initProgressDialog->appendText("Post-processing finished!");
04643 
04644         delete optimizer;
04645 }
04646 
04647 void MainWindow::deleteMemory()
04648 {
04649         QMessageBox::StandardButton button;
04650         if(_state == kMonitoring || _state == kMonitoringPaused)
04651         {
04652                 button = QMessageBox::question(this,
04653                                 tr("Deleting memory..."),
04654                                 tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
04655                                 QMessageBox::Yes|QMessageBox::No,
04656                                 QMessageBox::No);
04657         }
04658         else
04659         {
04660                 button = QMessageBox::question(this,
04661                                 tr("Deleting memory..."),
04662                                 tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
04663                                 QMessageBox::Yes|QMessageBox::No,
04664                                 QMessageBox::No);
04665         }
04666 
04667         if(button != QMessageBox::Yes)
04668         {
04669                 return;
04670         }
04671 
04672         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
04673         if(_state!=kDetecting)
04674         {
04675                 _databaseUpdated = false;
04676         }
04677         this->clearTheCache();
04678 }
04679 
04680 QString MainWindow::getWorkingDirectory() const
04681 {
04682         return _preferencesDialog->getWorkingDirectory();
04683 }
04684 
04685 void MainWindow::openWorkingDirectory()
04686 {
04687         QString filePath = _preferencesDialog->getWorkingDirectory();
04688 #if defined(Q_WS_MAC)
04689     QStringList args;
04690     args << "-e";
04691     args << "tell application \"Finder\"";
04692     args << "-e";
04693     args << "activate";
04694     args << "-e";
04695     args << "select POSIX file \""+filePath+"\"";
04696     args << "-e";
04697     args << "end tell";
04698     QProcess::startDetached("osascript", args);
04699 #elif defined(Q_WS_WIN)
04700     QStringList args;
04701     args << "/select," << QDir::toNativeSeparators(filePath);
04702     QProcess::startDetached("explorer", args);
04703 #else
04704     UERROR("Only works on Mac and Windows");
04705 #endif
04706 }
04707 
04708 void MainWindow::updateEditMenu()
04709 {
04710         // Update Memory delete database size
04711         if(_state != kMonitoring && _state != kMonitoringPaused && (!_openedDatabasePath.isEmpty() || !_newDatabasePath.isEmpty()))
04712         {
04713                 if(!_openedDatabasePath.isEmpty())
04714                 {
04715                         _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_openedDatabasePath.toStdString())/1000000));
04716                 }
04717                 else
04718                 {
04719                         _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_newDatabasePath.toStdString())/1000000));
04720                 }
04721         }
04722 }
04723 
04724 void MainWindow::selectStream()
04725 {
04726         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice);
04727 }
04728 
04729 void MainWindow::selectOpenni()
04730 {
04731         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
04732 }
04733 
04734 void MainWindow::selectFreenect()
04735 {
04736         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
04737 }
04738 
04739 void MainWindow::selectOpenniCv()
04740 {
04741         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
04742 }
04743 
04744 void MainWindow::selectOpenniCvAsus()
04745 {
04746         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
04747 }
04748 
04749 void MainWindow::selectOpenni2()
04750 {
04751         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
04752 }
04753 
04754 void MainWindow::selectFreenect2()
04755 {
04756         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
04757 }
04758 
04759 void MainWindow::selectStereoDC1394()
04760 {
04761         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
04762 }
04763 
04764 void MainWindow::selectStereoFlyCapture2()
04765 {
04766         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
04767 }
04768 void MainWindow::selectStereoZed()
04769 {
04770         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed);
04771 }
04772 
04773 void MainWindow::selectStereoUsb()
04774 {
04775         _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
04776 }
04777 
04778 void MainWindow::dumpTheMemory()
04779 {
04780         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
04781 }
04782 
04783 void MainWindow::dumpThePrediction()
04784 {
04785         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
04786 }
04787 
04788 void MainWindow::sendGoal()
04789 {
04790         UINFO("Sending a goal...");
04791         bool ok = false;
04792         QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
04793         if(ok && !text.isEmpty())
04794         {
04795                 _waypoints.clear();
04796                 _waypointsIndex = 0;
04797 
04798                 this->postGoal(text);
04799         }
04800 }
04801 
04802 void MainWindow::sendWaypoints()
04803 {
04804         UINFO("Sending waypoints...");
04805         bool ok = false;
04806         QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
04807         if(ok && !text.isEmpty())
04808         {
04809                 QStringList wp = text.split(' ');
04810                 if(wp.size() < 2)
04811                 {
04812                         QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
04813                 }
04814                 else
04815                 {
04816                         _waypoints = wp;
04817                         _waypointsIndex = 0;
04818                         this->postGoal(_waypoints.at(_waypointsIndex));
04819                 }
04820         }
04821 }
04822 
04823 void MainWindow::postGoal(const QString & goal)
04824 {
04825         if(!goal.isEmpty())
04826         {
04827                 bool ok = false;
04828                 int id = goal.toInt(&ok);
04829                 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
04830                 UINFO("Posting event with goal %s", goal.toStdString().c_str());
04831                 if(ok)
04832                 {
04833                         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
04834                 }
04835                 else
04836                 {
04837                         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
04838                 }
04839         }
04840 }
04841 
04842 void MainWindow::cancelGoal()
04843 {
04844         UINFO("Cancelling goal...");
04845         _waypoints.clear();
04846         _waypointsIndex = 0;
04847         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
04848 }
04849 
04850 void MainWindow::label()
04851 {
04852         UINFO("Labelling current location...");
04853         bool ok = false;
04854         QString label = QInputDialog::getText(this, tr("Label current location"), tr("Label: "), QLineEdit::Normal, "", &ok);
04855         if(ok && !label.isEmpty())
04856         {
04857                 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdLabel, label.toStdString(), 0));
04858         }
04859 }
04860 
04861 void MainWindow::updateCacheFromDatabase()
04862 {
04863         QString dir = getWorkingDirectory();
04864         QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
04865         if(!path.isEmpty())
04866         {
04867                 updateCacheFromDatabase(path);
04868         }
04869 }
04870 
04871 void MainWindow::updateCacheFromDatabase(const QString & path)
04872 {
04873         if(!path.isEmpty())
04874         {
04875                 DBDriver * driver = DBDriver::create();
04876                 if(driver->openConnection(path.toStdString()))
04877                 {
04878                         UINFO("Update cache...");
04879                         _initProgressDialog->resetProgress();
04880                         _initProgressDialog->show();
04881                         _initProgressDialog->appendText(tr("Downloading the map from \"%1\" (without poses and links)...")
04882                                         .arg(path));
04883 
04884                         std::set<int> ids;
04885                         driver->getAllNodeIds(ids, true);
04886                         std::list<Signature*> signaturesList;
04887                         driver->loadSignatures(std::list<int>(ids.begin(), ids.end()), signaturesList);
04888                         std::map<int, Signature> signatures;
04889                         driver->loadNodeData(signaturesList);
04890                         for(std::list<Signature *>::iterator iter=signaturesList.begin(); iter!=signaturesList.end(); ++iter)
04891                         {
04892                                 signatures.insert(std::make_pair((*iter)->id(), *(*iter)));
04893                                 delete *iter;
04894                         }
04895                         RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap);
04896                         processRtabmapEvent3DMap(event);
04897                 }
04898                 else
04899                 {
04900                         QMessageBox::warning(this, tr("Update cache"), tr("Failed to open database \"%1\"").arg(path));
04901                 }
04902                 delete driver;
04903         }
04904 }
04905 
04906 void MainWindow::downloadAllClouds()
04907 {
04908         QStringList items;
04909         items.append("Local map optimized");
04910         items.append("Local map not optimized");
04911         items.append("Global map optimized");
04912         items.append("Global map not optimized");
04913 
04914         bool ok;
04915         QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 2, false, &ok);
04916         if(ok)
04917         {
04918                 bool optimized=false, global=false;
04919                 if(item.compare("Local map optimized") == 0)
04920                 {
04921                         optimized = true;
04922                 }
04923                 else if(item.compare("Local map not optimized") == 0)
04924                 {
04925 
04926                 }
04927                 else if(item.compare("Global map optimized") == 0)
04928                 {
04929                         global=true;
04930                         optimized=true;
04931                 }
04932                 else if(item.compare("Global map not optimized") == 0)
04933                 {
04934                         global=true;
04935                 }
04936                 else
04937                 {
04938                         UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
04939                 }
04940 
04941                 UINFO("Download clouds...");
04942                 _initProgressDialog->resetProgress();
04943                 _initProgressDialog->show();
04944                 _initProgressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
04945                                 .arg(global?"true":"false").arg(optimized?"true":"false"));
04946                 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
04947         }
04948 }
04949 
04950 void MainWindow::downloadPoseGraph()
04951 {
04952         QStringList items;
04953         items.append("Local map optimized");
04954         items.append("Local map not optimized");
04955         items.append("Global map optimized");
04956         items.append("Global map not optimized");
04957 
04958         bool ok;
04959         QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 2, false, &ok);
04960         if(ok)
04961         {
04962                 bool optimized=false, global=false;
04963                 if(item.compare("Local map optimized") == 0)
04964                 {
04965                         optimized = true;
04966                 }
04967                 else if(item.compare("Local map not optimized") == 0)
04968                 {
04969 
04970                 }
04971                 else if(item.compare("Global map optimized") == 0)
04972                 {
04973                         global=true;
04974                         optimized=true;
04975                 }
04976                 else if(item.compare("Global map not optimized") == 0)
04977                 {
04978                         global=true;
04979                 }
04980                 else
04981                 {
04982                         UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
04983                 }
04984 
04985                 UINFO("Download the graph...");
04986                 _initProgressDialog->resetProgress();
04987                 _initProgressDialog->show();
04988                 _initProgressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
04989                                 .arg(global?"true":"false").arg(optimized?"true":"false"));
04990 
04991                 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
04992         }
04993 }
04994 
04995 void MainWindow::anchorCloudsToGroundTruth()
04996 {
04997         this->updateMapCloud(
04998                         std::map<int, Transform>(_currentPosesMap),
04999                         std::multimap<int, Link>(_currentLinksMap),
05000                         std::map<int, int>(_currentMapIds),
05001                         std::map<int, std::string>(_currentLabels),
05002                         std::map<int, Transform>(_currentGTPosesMap));
05003 }
05004 
05005 void MainWindow::clearTheCache()
05006 {
05007         _cachedSignatures.clear();
05008         _cachedMemoryUsage = 0;
05009         _cachedClouds.clear();
05010         _createdCloudsMemoryUsage = 0;
05011         _previousCloud.first = 0;
05012         _previousCloud.second.first.first.reset();
05013         _previousCloud.second.first.second.reset();
05014         _previousCloud.second.second.reset();
05015         _createdScans.clear();
05016         _gridLocalMaps.clear();
05017         _projectionLocalMaps.clear();
05018         _createdFeatures.clear();
05019         _cloudViewer->clear();
05020         _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
05021         _cloudViewer->clearTrajectory();
05022         _ui->widget_mapVisibility->clear();
05023         _currentPosesMap.clear();
05024         _currentGTPosesMap.clear();
05025         _currentLinksMap.clear();
05026         _currentMapIds.clear();
05027         _currentLabels.clear();
05028         _odometryCorrection = Transform::getIdentity();
05029         _lastOdomPose.setNull();
05030         //disable save cloud action
05031         _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
05032         _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
05033         _ui->actionPost_processing->setEnabled(false);
05034         _ui->actionSave_point_cloud->setEnabled(false);
05035         _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
05036         _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
05037         _ui->actionView_scans->setEnabled(false);
05038         _ui->actionExport_octomap->setEnabled(false);
05039         _ui->actionView_high_res_point_cloud->setEnabled(false);
05040         _likelihoodCurve->clear();
05041         _rawLikelihoodCurve->clear();
05042         _posteriorCurve->clear();
05043         _lastId = 0;
05044         _lastIds.clear();
05045         _firstStamp = 0.0f;
05046         _ui->label_stats_loopClosuresDetected->setText("0");
05047         _ui->label_stats_loopClosuresReactivatedDetected->setText("0");
05048         _ui->label_stats_loopClosuresRejected->setText("0");
05049         _refIds.clear();
05050         _loopClosureIds.clear();
05051         _ui->label_refId->clear();
05052         _ui->label_matchId->clear();
05053         _ui->graphicsView_graphView->clearAll();
05054         _ui->imageView_source->clear();
05055         _ui->imageView_loopClosure->clear();
05056         _ui->imageView_odometry->clear();
05057         _ui->imageView_source->setBackgroundColor(Qt::black);
05058         _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
05059         _ui->imageView_odometry->setBackgroundColor(Qt::black);
05060 #ifdef RTABMAP_OCTOMAP
05061         // re-create one if the resolution has changed
05062         UASSERT(_octomap != 0);
05063         delete _octomap;
05064         _octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
05065 #endif
05066 }
05067 
05068 void MainWindow::updateElapsedTime()
05069 {
05070         if(_state == kDetecting || _state == kMonitoring)
05071         {
05072                 QString format = "hh:mm:ss";
05073                 _ui->label_elapsedTime->setText((QTime().fromString(_ui->label_elapsedTime->text(), format).addMSecs(_elapsedTime->restart())).toString(format));
05074         }
05075 }
05076 
05077 void MainWindow::saveFigures()
05078 {
05079         QList<int> curvesPerFigure;
05080         QStringList curveNames;
05081         _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
05082 
05083         QStringList curvesPerFigureStr;
05084         for(int i=0; i<curvesPerFigure.size(); ++i)
05085         {
05086                 curvesPerFigureStr.append(QString::number(curvesPerFigure[i]));
05087         }
05088         for(int i=0; i<curveNames.size(); ++i)
05089         {
05090                 curveNames[i].replace(' ', '_');
05091         }
05092         _preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
05093         _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
05094 }
05095 
05096 void MainWindow::loadFigures()
05097 {
05098         QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
05099         QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
05100 
05101         if(!curvesPerFigure.isEmpty())
05102         {
05103                 QStringList curvesPerFigureList = curvesPerFigure.split(" ");
05104                 QStringList curvesNamesList = curveNames.split(" ");
05105 
05106                 int j=0;
05107                 for(int i=0; i<curvesPerFigureList.size(); ++i)
05108                 {
05109                         bool ok = false;
05110                         int count = curvesPerFigureList[i].toInt(&ok);
05111                         if(!ok)
05112                         {
05113                                 QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
05114                                 break;
05115                         }
05116                         else
05117                         {
05118                                 _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
05119                                 for(int k=1; k<count && j<curveNames.size(); ++k)
05120                                 {
05121                                         _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
05122                                 }
05123                         }
05124                 }
05125         }
05126 
05127 }
05128 
05129 void MainWindow::openPreferences()
05130 {
05131         _preferencesDialog->setMonitoringState(_state == kMonitoring || _state == kMonitoringPaused);
05132         _preferencesDialog->exec();
05133 }
05134 
05135 void MainWindow::openPreferencesSource()
05136 {
05137         _preferencesDialog->setCurrentPanelToSource();
05138         openPreferences();
05139         this->updateSelectSourceMenu();
05140 }
05141 
05142 void MainWindow::setDefaultViews()
05143 {
05144         _ui->dockWidget_posterior->setVisible(false);
05145         _ui->dockWidget_likelihood->setVisible(false);
05146         _ui->dockWidget_rawlikelihood->setVisible(false);
05147         _ui->dockWidget_statsV2->setVisible(false);
05148         _ui->dockWidget_console->setVisible(false);
05149         _ui->dockWidget_loopClosureViewer->setVisible(false);
05150         _ui->dockWidget_mapVisibility->setVisible(false);
05151         _ui->dockWidget_graphViewer->setVisible(false);
05152         _ui->dockWidget_odometry->setVisible(true);
05153         _ui->dockWidget_cloudViewer->setVisible(true);
05154         _ui->dockWidget_imageView->setVisible(true);
05155         _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused);
05156         _ui->toolBar_2->setVisible(true);
05157         _ui->statusbar->setVisible(false);
05158         this->setAspectRatio720p();
05159         _cloudViewer->resetCamera();
05160 }
05161 
05162 void MainWindow::selectScreenCaptureFormat(bool checked)
05163 {
05164         if(checked)
05165         {
05166                 QStringList items;
05167                 items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
05168                 bool ok;
05169                 QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
05170                 if(ok && !item.isEmpty())
05171                 {
05172                         if(item.compare("Synchronize with map update") == 0)
05173                         {
05174                                 _autoScreenCaptureOdomSync = false;
05175                         }
05176                         else
05177                         {
05178                                 _autoScreenCaptureOdomSync = true;
05179                         }
05180 
05181                         if(_state != kMonitoring && _state != kMonitoringPaused)
05182                         {
05183                                 int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM? Images will be saved on disk when clicking auto screen capture again."), QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
05184                                 if(r == QMessageBox::No || r == QMessageBox::Yes)
05185                                 {
05186                                         _autoScreenCaptureRAM = r == QMessageBox::Yes;
05187                                 }
05188                                 else
05189                                 {
05190                                         _ui->actionAuto_screen_capture->setChecked(false);
05191                                 }
05192                         }
05193                 }
05194                 else
05195                 {
05196                         _ui->actionAuto_screen_capture->setChecked(false);
05197                 }
05198         }
05199         else if(_autoScreenCaptureCachedImages.size())
05200         {
05201                 QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
05202                 QDir dir;
05203                 if(!dir.exists(targetDir))
05204                 {
05205                         dir.mkdir(targetDir);
05206                 }
05207                 targetDir += QDir::separator();
05208                 targetDir += "Main_window";
05209                 if(!dir.exists(targetDir))
05210                 {
05211                         dir.mkdir(targetDir);
05212                 }
05213                 targetDir += QDir::separator();
05214 
05215                 _initProgressDialog->resetProgress();
05216                 _initProgressDialog->show();
05217                 _initProgressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size());
05218                 int i=0;
05219                 for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end(); ++iter)
05220                 {
05221                         QPixmap figure;
05222                         figure.loadFromData(iter.value(), "PNG");
05223                         figure.save(targetDir + iter.key(), "PNG");
05224                         _initProgressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size()));
05225                         _initProgressDialog->incrementStep();
05226                 }
05227                 _autoScreenCaptureCachedImages.clear();
05228                 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
05229         }
05230 }
05231 
05232 void MainWindow::takeScreenshot()
05233 {
05234         QDesktopServices::openUrl(QUrl::fromLocalFile(this->captureScreen()));
05235 }
05236 
05237 void MainWindow::setAspectRatio(int w, int h)
05238 {
05239         QRect rect = this->geometry();
05240         if(h<100 && w<100)
05241         {
05242                 // it is a ratio
05243                 if(float(rect.width())/float(rect.height()) > float(w)/float(h))
05244                 {
05245                         rect.setWidth(w*(rect.height()/h));
05246                         rect.setHeight((rect.height()/h)*h);
05247                 }
05248                 else
05249                 {
05250                         rect.setHeight(h*(rect.width()/w));
05251                         rect.setWidth((rect.width()/w)*w);
05252                 }
05253         }
05254         else
05255         {
05256                 // it is absolute size
05257                 rect.setWidth(w);
05258                 rect.setHeight(h);
05259         }
05260         this->setGeometry(rect);
05261 }
05262 
05263 void MainWindow::setAspectRatio16_9()
05264 {
05265         this->setAspectRatio(16, 9);
05266 }
05267 
05268 void MainWindow::setAspectRatio16_10()
05269 {
05270         this->setAspectRatio(16, 10);
05271 }
05272 
05273 void MainWindow::setAspectRatio4_3()
05274 {
05275         this->setAspectRatio(4, 3);
05276 }
05277 
05278 void MainWindow::setAspectRatio240p()
05279 {
05280         this->setAspectRatio((240*16)/9, 240);
05281 }
05282 
05283 void MainWindow::setAspectRatio360p()
05284 {
05285         this->setAspectRatio((360*16)/9, 360);
05286 }
05287 
05288 void MainWindow::setAspectRatio480p()
05289 {
05290         this->setAspectRatio((480*16)/9, 480);
05291 }
05292 
05293 void MainWindow::setAspectRatio720p()
05294 {
05295         this->setAspectRatio((720*16)/9, 720);
05296 }
05297 
05298 void MainWindow::setAspectRatio1080p()
05299 {
05300         this->setAspectRatio((1080*16)/9, 1080);
05301 }
05302 
05303 void MainWindow::setAspectRatioCustom()
05304 {
05305         bool ok;
05306         int width = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Width (pixels):"), this->geometry().width(), 100, 10000, 100, &ok);
05307         if(ok)
05308         {
05309                 int height = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Height (pixels):"), this->geometry().height(), 100, 10000, 100, &ok);
05310                 if(ok)
05311                 {
05312                         this->setAspectRatio(width, height);
05313                 }
05314         }
05315 }
05316 
05317 void MainWindow::exportGridMap()
05318 {
05319         double gridCellSize = 0.05;
05320         bool ok;
05321         gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
05322         if(!ok)
05323         {
05324                 return;
05325         }
05326 
05327         std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
05328 
05329         // create the map
05330         float xMin=0.0f, yMin=0.0f;
05331         cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
05332                                 poses,
05333                                 _preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
05334                                 gridCellSize,
05335                                 xMin, yMin,
05336                                 0,
05337                                 _preferencesDialog->isGridMapEroded());
05338 
05339         if(!pixels.empty())
05340         {
05341                 cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
05342                 //convert to gray scaled map
05343                 for (int i = 0; i < pixels.rows; ++i)
05344                 {
05345                         for (int j = 0; j < pixels.cols; ++j)
05346                         {
05347                                 char v = pixels.at<char>(i, j);
05348                                 unsigned char gray;
05349                                 if(v == 0)
05350                                 {
05351                                         gray = 178;
05352                                 }
05353                                 else if(v == 100)
05354                                 {
05355                                         gray = 0;
05356                                 }
05357                                 else // -1
05358                                 {
05359                                         gray = 89;
05360                                 }
05361                                 map8U.at<unsigned char>(i, j) = gray;
05362                         }
05363                 }
05364 
05365                 QImage image = uCvMat2QImage(map8U, false);
05366 
05367                 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
05368                 if(!path.isEmpty())
05369                 {
05370                         if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
05371                         {
05372                                 //use png by default
05373                                 path += ".png";
05374                         }
05375 
05376                         QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
05377                         QPixmap::fromImage(img).save(path);
05378 
05379                         QDesktopServices::openUrl(QUrl::fromLocalFile(path));
05380                 }
05381         }
05382 }
05383 
05384 void MainWindow::exportScans()
05385 {
05386         if(_exportScansDialog->isVisible())
05387         {
05388                 return;
05389         }
05390 
05391         _exportScansDialog->exportScans(
05392                         _currentPosesMap,
05393                         _currentMapIds,
05394                         _cachedSignatures,
05395                         _createdScans,
05396                         _preferencesDialog->getWorkingDirectory());
05397 }
05398 
05399 void MainWindow::viewScans()
05400 {
05401         if(_exportScansDialog->isVisible())
05402         {
05403                 return;
05404         }
05405 
05406         _exportScansDialog->viewScans(
05407                         _ui->widget_mapVisibility->getVisiblePoses(),
05408                         _currentMapIds,
05409                         _cachedSignatures,
05410                         _createdScans,
05411                         _preferencesDialog->getWorkingDirectory());
05412 
05413 }
05414 
05415 void MainWindow::exportClouds()
05416 {
05417         if(_exportCloudsDialog->isVisible())
05418         {
05419                 return;
05420         }
05421 
05422         _exportCloudsDialog->exportClouds(
05423                         _ui->widget_mapVisibility->getVisiblePoses(),
05424                         _currentMapIds,
05425                         _cachedSignatures,
05426                         _cachedClouds,
05427                         _preferencesDialog->getWorkingDirectory(),
05428                         _preferencesDialog->getAllParameters());
05429 }
05430 
05431 void MainWindow::viewClouds()
05432 {
05433         if(_exportCloudsDialog->isVisible())
05434         {
05435                 return;
05436         }
05437 
05438         _exportCloudsDialog->viewClouds(
05439                         _currentPosesMap,
05440                         _currentMapIds,
05441                         _cachedSignatures,
05442                         _cachedClouds,
05443                         _preferencesDialog->getWorkingDirectory(),
05444                         _preferencesDialog->getAllParameters());
05445 
05446 }
05447 
05448 void MainWindow::exportOctomap()
05449 {
05450 #ifdef RTABMAP_OCTOMAP
05451         if(_octomap->octree()->size())
05452         {
05453                 QString path = QFileDialog::getSaveFileName(
05454                                 this,
05455                                 tr("Save File"),
05456                                 this->getWorkingDirectory()+"/"+"octomap.bt",
05457                                 tr("Octomap file (*.bt)"));
05458 
05459                 if(!path.isEmpty())
05460                 {
05461                         if(_octomap->writeBinary(path.toStdString()))
05462                         {
05463                                 QMessageBox::information(this,
05464                                                 tr("Export octomap..."),
05465                                                 tr("Octomap successfully saved to \"%1\".")
05466                                                 .arg(path));
05467                         }
05468                         else
05469                         {
05470                                 QMessageBox::information(this,
05471                                                 tr("Export octomap..."),
05472                                                 tr("Failed to save octomap to \"%1\"!")
05473                                                 .arg(path));
05474                         }
05475                 }
05476         }
05477         else
05478         {
05479                 UERROR("Empty octomap.");
05480         }
05481 #else
05482         UERROR("Cannot export octomap, RTAB-Map is not built with it.");
05483 #endif
05484 }
05485 
05486 void MainWindow::exportImages()
05487 {
05488         if(_cachedSignatures.empty())
05489         {
05490                 QMessageBox::warning(this, tr("Export images..."), tr("Cannot export images, the cache is empty!"));
05491                 return;
05492         }
05493         std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
05494 
05495         if(poses.empty())
05496         {
05497                 QMessageBox::warning(this, tr("Export images..."), tr("There is no map!"));
05498                 return;
05499         }
05500 
05501         QStringList formats;
05502         formats.push_back("jpg");
05503         formats.push_back("png");
05504         bool ok;
05505         QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
05506         if(!ok)
05507         {
05508                 return;
05509         }
05510 
05511         QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), this->getWorkingDirectory());
05512         if(!path.isNull())
05513         {
05514                 SensorData data;
05515                 if(_cachedSignatures.contains(poses.rbegin()->first))
05516                 {
05517                         data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
05518                         data.uncompressData();
05519                 }
05520                 if(!data.imageRaw().empty() && !data.rightRaw().empty())
05521                 {
05522                         QDir dir;
05523                         dir.mkdir(QString("%1/left").arg(path));
05524                         dir.mkdir(QString("%1/right").arg(path));
05525                         if(data.stereoCameraModel().isValidForProjection())
05526                         {
05527                                 std::string cameraName = "calibration";
05528                                 StereoCameraModel model(
05529                                                 cameraName,
05530                                                 data.imageRaw().size(),
05531                                                 data.stereoCameraModel().left().K(),
05532                                                 data.stereoCameraModel().left().D(),
05533                                                 data.stereoCameraModel().left().R(),
05534                                                 data.stereoCameraModel().left().P(),
05535                                                 data.rightRaw().size(),
05536                                                 data.stereoCameraModel().right().K(),
05537                                                 data.stereoCameraModel().right().D(),
05538                                                 data.stereoCameraModel().right().R(),
05539                                                 data.stereoCameraModel().right().P(),
05540                                                 data.stereoCameraModel().R(),
05541                                                 data.stereoCameraModel().T(),
05542                                                 data.stereoCameraModel().E(),
05543                                                 data.stereoCameraModel().F(),
05544                                                 data.stereoCameraModel().left().localTransform());
05545                                 if(model.save(path.toStdString()))
05546                                 {
05547                                         UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
05548                                 }
05549                                 else
05550                                 {
05551                                         UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
05552                                 }
05553                         }
05554                 }
05555                 else if(!data.imageRaw().empty())
05556                 {
05557                         if(!data.depthRaw().empty())
05558                         {
05559                                 QDir dir;
05560                                 dir.mkdir(QString("%1/rgb").arg(path));
05561                                 dir.mkdir(QString("%1/depth").arg(path));
05562                         }
05563 
05564                         if(data.cameraModels().size() > 1)
05565                         {
05566                                 UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
05567                         }
05568                         else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
05569                         {
05570                                 std::string cameraName = "calibration";
05571                                 CameraModel model(cameraName,
05572                                                 data.imageRaw().size(),
05573                                                 data.cameraModels().front().K(),
05574                                                 data.cameraModels().front().D(),
05575                                                 data.cameraModels().front().R(),
05576                                                 data.cameraModels().front().P(),
05577                                                 data.cameraModels().front().localTransform());
05578                                 if(model.save(path.toStdString()))
05579                                 {
05580                                         UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
05581                                 }
05582                                 else
05583                                 {
05584                                         UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
05585                                 }
05586                         }
05587                 }
05588                 else
05589                 {
05590                         QMessageBox::warning(this,
05591                                         tr("Export images..."),
05592                                         tr("Data in the cache don't seem to have images (tested node %1). Calibration file will not be saved. Try refreshing the cache (with clouds).").arg(poses.rbegin()->first));
05593                 }
05594         }
05595 
05596         _initProgressDialog->resetProgress();
05597         _initProgressDialog->show();
05598         _initProgressDialog->setMaximumSteps(_cachedSignatures.size());
05599 
05600         unsigned int saved = 0;
05601         for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
05602         {
05603                 int id = iter->first;
05604                 SensorData data;
05605                 if(_cachedSignatures.contains(iter->first))
05606                 {
05607                         data = _cachedSignatures.value(iter->first).sensorData();
05608                         data.uncompressData();
05609                 }
05610                 QString info;
05611                 bool warn = false;
05612                 if(!data.imageRaw().empty() && !data.rightRaw().empty())
05613                 {
05614                         cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
05615                         cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
05616                         info = tr("Saved left/%1.%2 and right/%1.%2.").arg(id).arg(ext);
05617                 }
05618                 else if(!data.imageRaw().empty() && !data.depthRaw().empty())
05619                 {
05620                         cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
05621                         cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw());
05622                         info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
05623                 }
05624                 else if(!data.imageRaw().empty())
05625                 {
05626                         cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
05627                         info = tr("Saved %1.%2.").arg(id).arg(ext);
05628                 }
05629                 else
05630                 {
05631                         info = tr("No images saved for node %1!").arg(id);
05632                         warn = true;
05633                 }
05634                 saved += warn?0:1;
05635                 _initProgressDialog->appendText(info, !warn?Qt::black:Qt::darkYellow);
05636                 _initProgressDialog->incrementStep();
05637                 QApplication::processEvents();
05638 
05639         }
05640         if(saved!=poses.size())
05641         {
05642                 _initProgressDialog->setAutoClose(false);
05643                 _initProgressDialog->appendText(tr("%1 images of %2 saved to \"%3\".").arg(saved).arg(poses.size()).arg(path));
05644         }
05645         else
05646         {
05647                 _initProgressDialog->appendText(tr("%1 images saved to \"%2\".").arg(saved).arg(path));
05648         }
05649 
05650         _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
05651 }
05652 
05653 void MainWindow::exportBundlerFormat()
05654 {
05655         std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
05656 
05657         std::map<int, Transform> poses;
05658         for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
05659         {
05660                 if(_cachedSignatures.contains(iter->first))
05661                 {
05662                         if(_cachedSignatures[iter->first].sensorData().imageRaw().empty() &&
05663                            _cachedSignatures[iter->first].sensorData().imageCompressed().empty())
05664                         {
05665                                 UWARN("Missing image in cache for node %d", iter->first);
05666                         }
05667                         else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
05668                                  _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection())
05669                         {
05670                                 poses.insert(*iter);
05671                         }
05672                         else
05673                         {
05674                                 UWARN("Missing calibration for node %d", iter->first);
05675                         }
05676                 }
05677                 else
05678                 {
05679                         UWARN("Did not find node %d in cache", iter->first);
05680                 }
05681         }
05682 
05683         if(poses.size())
05684         {
05685                 QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _preferencesDialog->getWorkingDirectory());
05686                 if(!path.isEmpty())
05687                 {
05688                         // export cameras and images
05689                         QFile fileOut(path+QDir::separator()+"cameras.out");
05690                         QFile fileList(path+QDir::separator()+"list.txt");
05691                         QDir(path).mkdir("images");
05692                         if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
05693                         {
05694                                 if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
05695                                 {
05696                                         QTextStream out(&fileOut);
05697                                         QTextStream list(&fileList);
05698                                         out << "# Bundle file v0.3\n";
05699                                         out << poses.size() << " 0\n";
05700 
05701                                         for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
05702                                         {
05703                                                 QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
05704                                                 list << p << "\n";
05705                                                 p = path+QDir::separator()+p;
05706                                                 cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw();
05707                                                 if(image.empty())
05708                                                 {
05709                                                         _cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
05710                                                 }
05711 
05712                                                 if(cv::imwrite(p.toStdString(), image))
05713                                                 {
05714                                                         UINFO("saved image %s", p.toStdString().c_str());
05715                                                 }
05716                                                 else
05717                                                 {
05718                                                         UERROR("Failed to save image %s", p.toStdString().c_str());
05719                                                 }
05720 
05721                                                 Transform localTransform;
05722                                                 if(_cachedSignatures[iter->first].sensorData().cameraModels().size())
05723                                                 {
05724                                                         out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
05725                                                         localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
05726                                                 }
05727                                                 else
05728                                                 {
05729                                                         out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
05730                                                         localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
05731                                                 }
05732 
05733                                                 Transform rotation(0,-1,0,0,
05734                                                                            0,0,1,0,
05735                                                                            -1,0,0,0);
05736 
05737                                                 Transform R = rotation*iter->second.rotation().inverse();
05738 
05739                                                 out << R.r11() << " " << R.r12() << " " << R.r13() << "\n";
05740                                                 out << R.r21() << " " << R.r22() << " " << R.r23() << "\n";
05741                                                 out << R.r31() << " " << R.r32() << " " << R.r33() << "\n";
05742 
05743                                                 Transform t = R * iter->second.translation();
05744                                                 t.x() *= -1.0f;
05745                                                 t.y() *= -1.0f;
05746                                                 t.z() *= -1.0f;
05747                                                 out << t.x() << " " << t.y() << " " << t.z() << "\n";
05748                                         }
05749 
05750                                         QMessageBox::question(this,
05751                                                         tr("Exporting cameras in Bundler format..."),
05752                                                         tr("%1 cameras/images exported to directory \"%2\".").arg(poses.size()).arg(path));
05753                                         fileList.close();
05754                                 }
05755                                 fileOut.close();
05756                         }
05757                 }
05758         }
05759         else
05760         {
05761                 QMessageBox::warning(this, tr("Exporting cameras..."), tr("No poses exported because of missing images. Try refreshing the cache (with clouds)."));
05762         }
05763 }
05764 
05765 void MainWindow::resetOdometry()
05766 {
05767         UINFO("reset odometry");
05768         this->post(new OdometryResetEvent());
05769 }
05770 
05771 void MainWindow::triggerNewMap()
05772 {
05773         UINFO("trigger a new map");
05774         this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
05775 }
05776 
05777 void MainWindow::dataRecorder()
05778 {
05779         if(_dataRecorder == 0)
05780         {
05781                 QString path = QFileDialog::getSaveFileName(this, tr("Save to..."), _preferencesDialog->getWorkingDirectory()+"/output.db", "RTAB-Map database (*.db)");
05782                 if(!path.isEmpty())
05783                 {
05784                         int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
05785 
05786                         if(r == QMessageBox::No || r == QMessageBox::Yes)
05787                         {
05788                                 bool recordInRAM = r == QMessageBox::Yes;
05789 
05790                                 _dataRecorder = new DataRecorder(this);
05791                                 _dataRecorder->setWindowFlags(Qt::Dialog);
05792                                 _dataRecorder->setAttribute(Qt::WA_DeleteOnClose, true);
05793                                 _dataRecorder->setWindowTitle(tr("Data recorder (%1)").arg(path));
05794 
05795                                 if(_dataRecorder->init(path, recordInRAM))
05796                                 {
05797                                         this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed()));
05798                                         _dataRecorder->show();
05799                                         _dataRecorder->registerToEventsManager();
05800                                         if(_camera)
05801                                         {
05802                                                 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
05803                                         }
05804                                         _ui->actionData_recorder->setEnabled(false);
05805                                 }
05806                                 else
05807                                 {
05808                                         QMessageBox::warning(this, tr(""), tr("Cannot initialize the data recorder!"));
05809                                         UERROR("Cannot initialize the data recorder!");
05810                                         delete _dataRecorder;
05811                                         _dataRecorder = 0;
05812                                 }
05813                         }
05814                 }
05815         }
05816         else
05817         {
05818                 UERROR("Only one recorder at the same time.");
05819         }
05820 }
05821 
05822 void MainWindow::dataRecorderDestroyed()
05823 {
05824         _ui->actionData_recorder->setEnabled(true);
05825         _dataRecorder = 0;
05826 }
05827 
05828 //END ACTIONS
05829 
05830 // STATES
05831 
05832 // in monitoring state, only some actions are enabled
05833 void MainWindow::setMonitoringState(bool pauseChecked)
05834 {
05835         this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
05836 }
05837 
05838 // Must be called by the GUI thread, use signal StateChanged()
05839 void MainWindow::changeState(MainWindow::State newState)
05840 {
05841         bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
05842         _ui->label_source->setVisible(!monitoring);
05843         _ui->label_stats_source->setVisible(!monitoring);
05844         _ui->actionNew_database->setVisible(!monitoring);
05845         _ui->actionOpen_database->setVisible(!monitoring);
05846         _ui->actionClose_database->setVisible(!monitoring);
05847         _ui->actionEdit_database->setVisible(!monitoring);
05848         _ui->actionStart->setVisible(!monitoring);
05849         _ui->actionStop->setVisible(!monitoring);
05850         _ui->actionDump_the_memory->setVisible(!monitoring);
05851         _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
05852         _ui->actionGenerate_map->setVisible(!monitoring);
05853         _ui->actionUpdate_cache_from_database->setVisible(monitoring);
05854         _ui->actionData_recorder->setVisible(!monitoring);
05855         _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
05856         _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
05857         _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
05858         bool wasMonitoring = _state==kMonitoring || _state == kMonitoringPaused;
05859         if(wasMonitoring != monitoring)
05860         {
05861                 _ui->toolBar->setVisible(!monitoring);
05862                 _ui->toolBar->toggleViewAction()->setVisible(!monitoring);
05863         }
05864         QList<QAction*> actions = _ui->menuTools->actions();
05865         for(int i=0; i<actions.size(); ++i)
05866         {
05867                 if(actions.at(i)->isSeparator())
05868                 {
05869                         actions.at(i)->setVisible(!monitoring);
05870                 }
05871         }
05872         actions = _ui->menuFile->actions();
05873         if(actions.size()==16)
05874         {
05875                 if(actions.at(2)->isSeparator())
05876                 {
05877                         actions.at(2)->setVisible(!monitoring);
05878                 }
05879                 else
05880                 {
05881                         UWARN("Menu File separators have not the same order.");
05882                 }
05883                 if(actions.at(12)->isSeparator())
05884                 {
05885                         actions.at(12)->setVisible(!monitoring);
05886                 }
05887                 else
05888                 {
05889                         UWARN("Menu File separators have not the same order.");
05890                 }
05891         }
05892         else
05893         {
05894                 UWARN("Menu File actions size has changed (%d)", actions.size());
05895         }
05896         actions = _ui->menuProcess->actions();
05897         if(actions.size()>=2)
05898         {
05899                 if(actions.at(1)->isSeparator())
05900                 {
05901                         actions.at(1)->setVisible(!monitoring);
05902                 }
05903                 else
05904                 {
05905                         UWARN("Menu File separators have not the same order.");
05906                 }
05907         }
05908         else
05909         {
05910                 UWARN("Menu File separators have not the same order.");
05911         }
05912 
05913         _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
05914 
05915         switch (newState)
05916         {
05917         case kIdle: // RTAB-Map is not initialized yet
05918                 _ui->actionNew_database->setEnabled(true);
05919                 _ui->actionOpen_database->setEnabled(true);
05920                 _ui->actionClose_database->setEnabled(false);
05921                 _ui->actionEdit_database->setEnabled(true);
05922                 _ui->actionStart->setEnabled(false);
05923                 _ui->actionPause->setEnabled(false);
05924                 _ui->actionPause->setChecked(false);
05925                 _ui->actionPause->setToolTip(tr("Pause"));
05926                 _ui->actionStop->setEnabled(false);
05927                 _ui->actionPause_on_match->setEnabled(true);
05928                 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05929                 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05930                 _ui->actionDump_the_memory->setEnabled(false);
05931                 _ui->actionDump_the_prediction_matrix->setEnabled(false);
05932                 _ui->actionDelete_memory->setEnabled(false);
05933                 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05934                 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
05935                 _ui->actionGenerate_map->setEnabled(false);
05936                 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
05937                 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
05938                 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
05939                 _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
05940                 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
05941                 _ui->actionView_scans->setEnabled(!_createdScans.empty());
05942 #ifdef RTABMAP_OCTOMAP
05943                 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
05944 #else
05945                 _ui->actionExport_octomap->setEnabled(false);
05946 #endif
05947                 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
05948                 _ui->actionDownload_all_clouds->setEnabled(false);
05949                 _ui->actionDownload_graph->setEnabled(false);
05950                 _ui->menuSelect_source->setEnabled(true);
05951                 _ui->actionLabel_current_location->setEnabled(false);
05952                 _ui->actionSend_goal->setEnabled(false);
05953                 _ui->actionCancel_goal->setEnabled(false);
05954                 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
05955                 _ui->actionTrigger_a_new_map->setEnabled(false);
05956                 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
05957                 _ui->statusbar->clearMessage();
05958                 _state = newState;
05959                 _oneSecondTimer->stop();
05960                 break;
05961 
05962         case kApplicationClosing:
05963         case kClosing:
05964                 _ui->actionStart->setEnabled(false);
05965                 _ui->actionPause->setEnabled(false);
05966                 _ui->actionStop->setEnabled(false);
05967                 _state = newState;
05968                 break;
05969 
05970         case kInitializing:
05971                 _ui->actionNew_database->setEnabled(false);
05972                 _ui->actionOpen_database->setEnabled(false);
05973                 _ui->actionClose_database->setEnabled(false);
05974                 _ui->actionEdit_database->setEnabled(false);
05975                 _state = newState;
05976                 break;
05977 
05978         case kInitialized:
05979                 _ui->actionNew_database->setEnabled(false);
05980                 _ui->actionOpen_database->setEnabled(false);
05981                 _ui->actionClose_database->setEnabled(true);
05982                 _ui->actionEdit_database->setEnabled(false);
05983                 _ui->actionStart->setEnabled(true);
05984                 _ui->actionPause->setEnabled(false);
05985                 _ui->actionPause->setChecked(false);
05986                 _ui->actionPause->setToolTip(tr("Pause"));
05987                 _ui->actionStop->setEnabled(false);
05988                 _ui->actionPause_on_match->setEnabled(true);
05989                 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05990                 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05991                 _ui->actionDump_the_memory->setEnabled(true);
05992                 _ui->actionDump_the_prediction_matrix->setEnabled(true);
05993                 _ui->actionDelete_memory->setEnabled(true);
05994                 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05995                 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
05996                 _ui->actionGenerate_map->setEnabled(true);
05997                 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
05998                 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
05999                 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
06000                 _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
06001                 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
06002                 _ui->actionView_scans->setEnabled(!_createdScans.empty());
06003 #ifdef RTABMAP_OCTOMAP
06004                 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
06005 #else
06006                 _ui->actionExport_octomap->setEnabled(false);
06007 #endif
06008                 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
06009                 _ui->actionDownload_all_clouds->setEnabled(true);
06010                 _ui->actionDownload_graph->setEnabled(true);
06011                 _ui->menuSelect_source->setEnabled(true);
06012                 _ui->actionLabel_current_location->setEnabled(true);
06013                 _ui->actionSend_goal->setEnabled(true);
06014                 _ui->actionCancel_goal->setEnabled(true);
06015                 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
06016                 _ui->actionTrigger_a_new_map->setEnabled(true);
06017                 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
06018                 _ui->statusbar->clearMessage();
06019                 _state = newState;
06020                 _oneSecondTimer->stop();
06021                 break;
06022 
06023         case kStartingDetection:
06024                 _ui->actionStart->setEnabled(false);
06025                 _state = newState;
06026                 break;
06027 
06028         case kDetecting:
06029                 _ui->actionNew_database->setEnabled(false);
06030                 _ui->actionOpen_database->setEnabled(false);
06031                 _ui->actionClose_database->setEnabled(false);
06032                 _ui->actionEdit_database->setEnabled(false);
06033                 _ui->actionStart->setEnabled(false);
06034                 _ui->actionPause->setEnabled(true);
06035                 _ui->actionPause->setChecked(false);
06036                 _ui->actionPause->setToolTip(tr("Pause"));
06037                 _ui->actionStop->setEnabled(true);
06038                 _ui->actionPause_on_match->setEnabled(true);
06039                 _ui->actionPause_on_local_loop_detection->setEnabled(true);
06040                 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
06041                 _ui->actionDump_the_memory->setEnabled(false);
06042                 _ui->actionDump_the_prediction_matrix->setEnabled(false);
06043                 _ui->actionDelete_memory->setEnabled(false);
06044                 _ui->actionPost_processing->setEnabled(false);
06045                 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
06046                 _ui->actionGenerate_map->setEnabled(false);
06047                 _ui->menuExport_poses->setEnabled(false);
06048                 _ui->actionSave_point_cloud->setEnabled(false);
06049                 _ui->actionView_high_res_point_cloud->setEnabled(false);
06050                 _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
06051                 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
06052                 _ui->actionView_scans->setEnabled(false);
06053                 _ui->actionExport_octomap->setEnabled(false);
06054                 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
06055                 _ui->actionDownload_all_clouds->setEnabled(false);
06056                 _ui->actionDownload_graph->setEnabled(false);
06057                 _ui->menuSelect_source->setEnabled(false);
06058                 _ui->actionLabel_current_location->setEnabled(true);
06059                 _ui->actionSend_goal->setEnabled(true);
06060                 _ui->actionCancel_goal->setEnabled(true);
06061                 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
06062                 _ui->actionTrigger_a_new_map->setEnabled(true);
06063                 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
06064                 _ui->statusbar->showMessage(tr("Detecting..."));
06065                 _state = newState;
06066                 _ui->label_elapsedTime->setText("00:00:00");
06067                 _elapsedTime->start();
06068                 _oneSecondTimer->start();
06069 
06070                 _databaseUpdated = true; // if a new database is used, it won't be empty anymore...
06071 
06072                 if(_camera)
06073                 {
06074                         _camera->start();
06075                 }
06076                 break;
06077 
06078         case kPaused:
06079                 if(_state == kPaused)
06080                 {
06081                         _ui->actionPause->setToolTip(tr("Pause"));
06082                         _ui->actionPause->setChecked(false);
06083                         _ui->statusbar->showMessage(tr("Detecting..."));
06084                         _ui->actionDump_the_memory->setEnabled(false);
06085                         _ui->actionDump_the_prediction_matrix->setEnabled(false);
06086                         _ui->actionDelete_memory->setEnabled(false);
06087                         _ui->actionPost_processing->setEnabled(false);
06088                         _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
06089                         _ui->actionGenerate_map->setEnabled(false);
06090                         _ui->menuExport_poses->setEnabled(false);
06091                         _ui->actionSave_point_cloud->setEnabled(false);
06092                         _ui->actionView_high_res_point_cloud->setEnabled(false);
06093                         _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
06094                         _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
06095                         _ui->actionView_scans->setEnabled(false);
06096                         _ui->actionExport_octomap->setEnabled(false);
06097                         _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
06098                         _ui->actionDownload_all_clouds->setEnabled(false);
06099                         _ui->actionDownload_graph->setEnabled(false);
06100                         _state = kDetecting;
06101                         _elapsedTime->start();
06102                         _oneSecondTimer->start();
06103 
06104                         if(_camera)
06105                         {
06106                                 _camera->start();
06107                         }
06108                 }
06109                 else if(_state == kDetecting)
06110                 {
06111                         _ui->actionPause->setToolTip(tr("Continue (shift-click for step-by-step)"));
06112                         _ui->actionPause->setChecked(true);
06113                         _ui->statusbar->showMessage(tr("Paused..."));
06114                         _ui->actionDump_the_memory->setEnabled(true);
06115                         _ui->actionDump_the_prediction_matrix->setEnabled(true);
06116                         _ui->actionDelete_memory->setEnabled(false);
06117                         _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
06118                         _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
06119                         _ui->actionGenerate_map->setEnabled(true);
06120                         _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
06121                         _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !