00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
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
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
00191
00192
00193
00194
00195
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
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
00220 {
00221 _preferencesDialog = new PreferencesDialog(this);
00222 }
00223
00224 _preferencesDialog->setObjectName("PreferencesDialog");
00225 _preferencesDialog->init();
00226
00227
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
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
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
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
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
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
00377
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
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
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
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
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
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
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
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
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
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
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
00608
00609
00610
00611
00612
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
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;
00775 OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
00776 emit odometryReceived(tmp, false);
00777 }
00778 else
00779 {
00780
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;
00797 emit odometryReceived(*odomEvent, false);
00798 }
00799 else
00800 {
00801
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
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
00855
00856
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");
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
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
00948 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
00949 output = util3d::extractIndices(cloud, indices, false, true);
00950
00951
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
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
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
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
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
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
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
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
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);
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);
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
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);
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
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
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
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
01357 Signature signature;
01358 if(uContains(stat.getSignatures(), stat.refImageId()))
01359 {
01360 signature = stat.getSignatures().at(stat.refImageId());
01361 signature.sensorData().uncompressData();
01362
01363 if(!smallMovement)
01364 {
01365
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
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
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
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
01497 loopSignature = iter.value();
01498 loopSignature.sensorData().uncompressData();
01499 }
01500 }
01501 }
01502 _refIds.push_back(stat.refImageId());
01503 _loopClosureIds.push_back(matchId);
01504
01505
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
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
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
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
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
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
01590
01591 UTimer timerVis;
01592
01593
01594 if(stat.poses().size())
01595 {
01596
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
01656 if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
01657 !stat.loopClosureTransform().isNull() &&
01658 !loopSignature.sensorData().imageRaw().empty())
01659 {
01660
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
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
01785 if(_preferencesDialog->isPosteriorGraphView() &&
01786 stat.posterior().size())
01787 {
01788 _ui->graphicsView_graphView->updatePosterior(stat.posterior());
01789 }
01790
01791 _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
01792 if(stat.localPath().size() == 0)
01793 {
01794
01795 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
01796 }
01797
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,
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
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;
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
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
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
01952 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
01953 if(viewerClouds.contains(cloudName))
01954 {
01955
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
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
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
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
02033 std::string featuresName = uFormat("features%d", iter->first);
02034 if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
02035 {
02036 if(viewerClouds.contains(featuresName))
02037 {
02038
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
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
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
02115 _cloudViewer->removeAllGraphs();
02116 _cloudViewer->removeCloud("graph_nodes");
02117 if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
02118 {
02119
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
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
02136 for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
02137 {
02138 int mapId = -100;
02139
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
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
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
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
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
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
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
02379 if(indices->size() && _preferencesDialog->getMapVoxel() > 0.0)
02380 {
02381 cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getMapVoxel());
02382
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
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
02419
02420
02421
02422 if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
02423 {
02424
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
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
02484
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
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
02599 if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
02600 {
02601 voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
02602 }
02603
02604
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
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
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
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
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
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
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
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
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
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 }
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);
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
03374 _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
03375 _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
03376
03377
03378 if(uContains(parameters, Parameters::kRGBDLocalRadius()))
03379 {
03380 _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
03381 }
03382 }
03383
03384
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
03407 color = Qt::magenta;
03408 }
03409 else if(_lastIds.contains(id))
03410 {
03411
03412 color = Qt::blue;
03413 }
03414 else if(id<=_lastId)
03415 {
03416
03417 color = Qt::red;
03418 }
03419 else if(refWords.count(id) > 1)
03420 {
03421
03422 color = Qt::yellow;
03423 }
03424 else
03425 {
03426
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
03443 color = Qt::magenta;
03444
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
03455 color = Qt::red;
03456 }
03457 else if(refWords.count(id) > 1)
03458 {
03459
03460 color = Qt::yellow;
03461 }
03462 else
03463 {
03464
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
03482 float scaleSource = _ui->imageView_source->viewScale();
03483 float scaleLoop = _ui->imageView_loopClosure->viewScale();
03484 UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
03485
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
03529 this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
03530 }
03531
03532 void MainWindow::moveEvent(QMoveEvent* anEvent)
03533 {
03534 if(this->isVisible())
03535 {
03536
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
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
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
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
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
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
03985 if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
03986 {
03987
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));
04034 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
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
04059 void MainWindow::pauseDetection()
04060 {
04061 if(_camera)
04062 {
04063 if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
04064 {
04065
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
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
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
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);
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
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)
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
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
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> >());
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
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
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
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
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
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
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
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
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
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
05829
05830
05831
05832
05833 void MainWindow::setMonitoringState(bool pauseChecked)
05834 {
05835 this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
05836 }
05837
05838
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:
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;
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() || !_cachedClouds.empty());
06122 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
06123 _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
06124 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
06125 _ui->actionView_scans->setEnabled(!_createdScans.empty());
06126 #ifdef RTABMAP_OCTOMAP
06127 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
06128 #else
06129 _ui->actionExport_octomap->setEnabled(false);
06130 #endif
06131 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
06132 _ui->actionDownload_all_clouds->setEnabled(true);
06133 _ui->actionDownload_graph->setEnabled(true);
06134 _state = kPaused;
06135 _oneSecondTimer->stop();
06136
06137
06138 if(_camera)
06139 {
06140 _camera->join(true);
06141 }
06142 }
06143 break;
06144 case kMonitoring:
06145 _ui->actionPause->setEnabled(true);
06146 _ui->actionPause->setChecked(false);
06147 _ui->actionPause->setToolTip(tr("Pause"));
06148 _ui->actionPause_on_match->setEnabled(true);
06149 _ui->actionPause_on_local_loop_detection->setEnabled(true);
06150 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
06151 _ui->actionReset_Odometry->setEnabled(true);
06152 _ui->actionPost_processing->setEnabled(false);
06153 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
06154 _ui->menuExport_poses->setEnabled(false);
06155 _ui->actionSave_point_cloud->setEnabled(false);
06156 _ui->actionView_high_res_point_cloud->setEnabled(false);
06157 _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
06158 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
06159 _ui->actionView_scans->setEnabled(false);
06160 _ui->actionExport_octomap->setEnabled(false);
06161 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
06162 _ui->actionDelete_memory->setEnabled(true);
06163 _ui->actionDownload_all_clouds->setEnabled(true);
06164 _ui->actionDownload_graph->setEnabled(true);
06165 _ui->actionTrigger_a_new_map->setEnabled(true);
06166 _ui->actionLabel_current_location->setEnabled(true);
06167 _ui->actionSend_goal->setEnabled(true);
06168 _ui->actionCancel_goal->setEnabled(true);
06169 _ui->statusbar->showMessage(tr("Monitoring..."));
06170 _state = newState;
06171 _elapsedTime->start();
06172 _oneSecondTimer->start();
06173 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResume));
06174 break;
06175 case kMonitoringPaused:
06176 _ui->actionPause->setToolTip(tr("Continue"));
06177 _ui->actionPause->setChecked(true);
06178 _ui->actionPause->setEnabled(true);
06179 _ui->actionPause_on_match->setEnabled(true);
06180 _ui->actionPause_on_local_loop_detection->setEnabled(true);
06181 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
06182 _ui->actionReset_Odometry->setEnabled(true);
06183 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
06184 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
06185 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
06186 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
06187 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
06188 _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
06189 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
06190 _ui->actionView_scans->setEnabled(!_createdScans.empty());
06191 #ifdef RTABMAP_OCTOMAP
06192 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
06193 #else
06194 _ui->actionExport_octomap->setEnabled(false);
06195 #endif
06196 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
06197 _ui->actionDelete_memory->setEnabled(true);
06198 _ui->actionDownload_all_clouds->setEnabled(true);
06199 _ui->actionDownload_graph->setEnabled(true);
06200 _ui->actionTrigger_a_new_map->setEnabled(true);
06201 _ui->actionLabel_current_location->setEnabled(true);
06202 _ui->actionSend_goal->setEnabled(true);
06203 _ui->actionCancel_goal->setEnabled(true);
06204 _ui->statusbar->showMessage(tr("Monitoring paused..."));
06205 _state = newState;
06206 _oneSecondTimer->stop();
06207 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause));
06208 break;
06209 default:
06210 break;
06211 }
06212
06213 }
06214
06215 }