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/Camera.h"
00033 #include "rtabmap/core/CameraThread.h"
00034 #include "rtabmap/core/CameraEvent.h"
00035 #include "rtabmap/core/DBReader.h"
00036 #include "rtabmap/core/Parameters.h"
00037 #include "rtabmap/core/ParamEvent.h"
00038 #include "rtabmap/core/Signature.h"
00039 #include "rtabmap/core/Memory.h"
00040
00041 #include "rtabmap/gui/ImageView.h"
00042 #include "rtabmap/gui/KeypointItem.h"
00043 #include "rtabmap/gui/DataRecorder.h"
00044 #include "rtabmap/gui/DatabaseViewer.h"
00045
00046 #include <rtabmap/utilite/UStl.h>
00047 #include <rtabmap/utilite/ULogger.h>
00048 #include <rtabmap/utilite/UEventsManager.h>
00049 #include <rtabmap/utilite/UFile.h>
00050 #include <rtabmap/utilite/UConversion.h>
00051 #include "utilite/UPlot.h"
00052 #include "rtabmap/gui/UCv2Qt.h"
00053
00054 #include "ExportCloudsDialog.h"
00055 #include "AboutDialog.h"
00056 #include "PdfPlot.h"
00057 #include "StatsToolBox.h"
00058 #include "DetailedProgressDialog.h"
00059 #include "PostProcessingDialog.h"
00060
00061 #include <QtGui/QCloseEvent>
00062 #include <QtGui/QPixmap>
00063 #include <QtCore/QDir>
00064 #include <QtCore/QFileInfo>
00065 #include <QMessageBox>
00066 #include <QFileDialog>
00067 #include <QGraphicsEllipseItem>
00068 #include <QDockWidget>
00069 #include <QtCore/QBuffer>
00070 #include <QtCore/QTimer>
00071 #include <QtCore/QTime>
00072 #include <QActionGroup>
00073 #include <QtCore/QThread>
00074 #include <QtGui/QDesktopServices>
00075 #include <QtCore/QStringList>
00076 #include <QtCore/QProcess>
00077 #include <QSplashScreen>
00078 #include <QInputDialog>
00079 #include <QToolButton>
00080
00081
00082 #include "rtabmap/core/CameraRGBD.h"
00083 #include "rtabmap/core/Odometry.h"
00084 #include "rtabmap/core/OdometryThread.h"
00085 #include "rtabmap/core/OdometryEvent.h"
00086 #include "rtabmap/core/util3d.h"
00087 #include "rtabmap/core/Graph.h"
00088 #include <pcl/visualization/cloud_viewer.h>
00089 #include <pcl/common/transforms.h>
00090 #include <pcl/common/common.h>
00091 #include <pcl/io/pcd_io.h>
00092 #include <pcl/io/ply_io.h>
00093 #include <pcl/io/vtk_io.h>
00094 #include <pcl/filters/filter.h>
00095 #include <pcl/search/kdtree.h>
00096
00097 #define LOG_FILE_NAME "LogRtabmap.txt"
00098 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
00099 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
00100 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m"
00101
00102 using namespace rtabmap;
00103
00104 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
00105
00106
00107 namespace rtabmap {
00108
00109 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
00110 QMainWindow(parent),
00111 _ui(0),
00112 _state(kIdle),
00113 _camera(0),
00114 _dbReader(0),
00115 _odomThread(0),
00116 _srcType(kSrcUndefined),
00117 _preferencesDialog(0),
00118 _aboutDialog(0),
00119 _exportDialog(0),
00120 _dataRecorder(0),
00121 _lastId(0),
00122 _processingStatistics(false),
00123 _odometryReceived(false),
00124 _newDatabasePath(""),
00125 _newDatabasePathOutput(""),
00126 _openedDatabasePath(""),
00127 _databaseUpdated(false),
00128 _odomImageShow(true),
00129 _odomImageDepthShow(false),
00130 _odometryCorrection(Transform::getIdentity()),
00131 _processingOdometry(false),
00132 _lastOdomInfoUpdateTime(0),
00133 _oneSecondTimer(0),
00134 _elapsedTime(0),
00135 _posteriorCurve(0),
00136 _likelihoodCurve(0),
00137 _rawLikelihoodCurve(0),
00138 _autoScreenCaptureOdomSync(false),
00139 _firstCall(true)
00140 {
00141 UDEBUG("");
00142
00143 initGuiResource();
00144
00145 QPixmap pixmap(":images/RTAB-Map.png");
00146 QSplashScreen splash(pixmap);
00147 splash.show();
00148 splash.showMessage(tr("Loading..."));
00149 QApplication::processEvents();
00150
00151
00152 _aboutDialog = new AboutDialog(this);
00153 _aboutDialog->setObjectName("AboutDialog");
00154 _exportDialog = new ExportCloudsDialog(this);
00155 _exportDialog->setObjectName("ExportCloudsDialog");
00156 _postProcessingDialog = new PostProcessingDialog(this);
00157 _postProcessingDialog->setObjectName("PostProcessingDialog");
00158
00159 _ui = new Ui_mainWindow();
00160 _ui->setupUi(this);
00161
00162 QString title("RTAB-Map[*]");
00163 this->setWindowTitle(title);
00164 this->setWindowIconText(tr("RTAB-Map"));
00165 this->setObjectName("MainWindow");
00166
00167
00168
00169 {
00170 _ui->dockWidget_posterior->setVisible(false);
00171 _ui->dockWidget_likelihood->setVisible(false);
00172 _ui->dockWidget_rawlikelihood->setVisible(false);
00173 _ui->dockWidget_statsV2->setVisible(false);
00174 _ui->dockWidget_console->setVisible(false);
00175 _ui->dockWidget_loopClosureViewer->setVisible(false);
00176 _ui->dockWidget_mapVisibility->setVisible(false);
00177 _ui->dockWidget_graphViewer->setVisible(false);
00178
00179
00180
00181 }
00182
00183 _ui->widget_mainWindow->setVisible(false);
00184
00185 if(prefDialog)
00186 {
00187 _preferencesDialog = prefDialog;
00188 _preferencesDialog->setParent(this, Qt::Dialog);
00189 }
00190 else
00191 {
00192 _preferencesDialog = new PreferencesDialog(this);
00193 }
00194 _preferencesDialog->setObjectName("PreferencesDialog");
00195 _preferencesDialog->init();
00196
00197
00198 _preferencesDialog->loadMainWindowState(this, _savedMaximized);
00199 _preferencesDialog->loadWindowGeometry(_preferencesDialog);
00200 _preferencesDialog->loadWindowGeometry(_exportDialog);
00201 _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
00202 _preferencesDialog->loadWindowGeometry(_aboutDialog);
00203 setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
00204
00205
00206 _oneSecondTimer = new QTimer(this);
00207 _oneSecondTimer->setInterval(1000);
00208 _elapsedTime = new QTime();
00209 _ui->label_elapsedTime->setText("00:00:00");
00210 connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
00211 _logEventTime = new QTime();
00212 _logEventTime->start();
00213
00214
00215 _ui->imageView_source->setBackgroundColor(Qt::black);
00216 _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
00217 _ui->imageView_odometry->setBackgroundColor(Qt::black);
00218 _ui->imageView_odometry->setAlpha(200);
00219 _preferencesDialog->loadWidgetState(_ui->imageView_source);
00220 _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
00221 _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
00222 _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
00223
00224 _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
00225 _ui->posteriorPlot->addCurve(_posteriorCurve, false);
00226 _ui->posteriorPlot->showLegend(false);
00227 _ui->posteriorPlot->setFixedYAxis(0,1);
00228 UPlotCurveThreshold * tc;
00229 tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
00230 connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
00231
00232 _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00233 _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
00234 _ui->likelihoodPlot->showLegend(false);
00235
00236 _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00237 _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
00238 _ui->rawLikelihoodPlot->showLegend(false);
00239
00240 _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
00241 _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
00242 _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
00243
00244 _initProgressDialog = new DetailedProgressDialog(this);
00245 _initProgressDialog->setWindowTitle(tr("Progress dialog"));
00246 _initProgressDialog->setMinimumWidth(800);
00247
00248 connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
00249
00250
00251 connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
00252 qRegisterMetaType<MainWindow::State>("MainWindow::State");
00253 connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
00254 connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
00255 qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
00256 connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
00257 qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
00258 connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
00259
00260
00261 _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
00262 _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
00263 _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
00264 _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
00265 _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
00266 _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
00267 _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
00268 _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
00269 _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
00270 _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
00271 _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
00272 _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
00273 _ui->toolBar->setWindowTitle(tr("File toolbar"));
00274 _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
00275 _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
00276 QAction * a = _ui->menuShow_view->addAction("Progress dialog");
00277 a->setCheckable(false);
00278 connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
00279
00280
00281 connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
00282 connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
00283 connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00284 connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
00285 connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
00286 connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
00287 connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
00288 connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
00289 connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
00290 connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
00291 connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
00292 connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
00293 connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
00294 connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
00295 connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap()));
00296 connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap()));
00297 connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap()));
00298 connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
00299 connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
00300 connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
00301 connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
00302 connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
00303 connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
00304 connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
00305 connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
00306 connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
00307 connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
00308 connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
00309 connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
00310 connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
00311 connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
00312 connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
00313 connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans()));
00314 connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
00315 connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans()));
00316 connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
00317 connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
00318 connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
00319 connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
00320 connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
00321
00322 _ui->actionPause->setShortcut(Qt::Key_Space);
00323 _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
00324 _ui->actionSave_point_cloud->setEnabled(false);
00325 _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
00326 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
00327 _ui->actionView_scans->setEnabled(false);
00328 _ui->actionView_high_res_point_cloud->setEnabled(false);
00329 _ui->actionReset_Odometry->setEnabled(false);
00330 _ui->actionPost_processing->setEnabled(false);
00331
00332 QToolButton* toolButton = new QToolButton(this);
00333 toolButton->setMenu(_ui->menuRGB_D_camera);
00334 toolButton->setPopupMode(QToolButton::InstantPopup);
00335 toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
00336 toolButton->setToolTip("Select sensor driver");
00337 _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
00338
00339 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
00340 connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
00341 #else
00342 _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
00343 #endif
00344
00345
00346 connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
00347 connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
00348 connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
00349 connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
00350 connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00351 connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00352 connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
00353 connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
00354 connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
00355 connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00356 connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00357 connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00358 connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
00359 connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
00360 connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
00361 _ui->actionFreenect->setEnabled(CameraFreenect::available());
00362 _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
00363 _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
00364 _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
00365 _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
00366 _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
00367 _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
00368 _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
00369 _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
00370 this->updateSelectSourceMenu();
00371
00372 connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
00373
00374 QActionGroup * modeGrp = new QActionGroup(this);
00375 modeGrp->addAction(_ui->actionSLAM_mode);
00376 modeGrp->addAction(_ui->actionLocalization_mode);
00377 _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
00378 _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
00379 connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00380 connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00381 connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
00382
00383
00384 qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
00385 connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
00386 qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
00387 connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
00388
00389 connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
00390 if(prefDialog == 0)
00391 {
00392 connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
00393 }
00394 connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00395 connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00396 connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00397 connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00398 connect(_ui->widget_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00399 connect(_exportDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00400 connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00401 connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00402 connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
00403 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00404 for(int i=0; i<dockWidgets.size(); ++i)
00405 {
00406 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
00407 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00408 }
00409
00410 _ui->dockWidget_posterior->installEventFilter(this);
00411 _ui->dockWidget_likelihood->installEventFilter(this);
00412 _ui->dockWidget_rawlikelihood->installEventFilter(this);
00413 _ui->dockWidget_statsV2->installEventFilter(this);
00414 _ui->dockWidget_console->installEventFilter(this);
00415 _ui->dockWidget_loopClosureViewer->installEventFilter(this);
00416 _ui->dockWidget_mapVisibility->installEventFilter(this);
00417 _ui->dockWidget_graphViewer->installEventFilter(this);
00418 _ui->dockWidget_odometry->installEventFilter(this);
00419 _ui->dockWidget_cloudViewer->installEventFilter(this);
00420 _ui->dockWidget_imageView->installEventFilter(this);
00421
00422
00423 connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
00424 connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
00425 connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
00426 connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
00427 connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
00428 connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
00429
00430
00431 qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
00432 connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
00433
00434 qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
00435 qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
00436 connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
00437
00438 connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
00439
00440
00441 this->changeState(kIdle);
00442 this->applyPrefSettings(PreferencesDialog::kPanelAll);
00443
00444 _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00445 _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00446 _ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00447 _preferencesDialog->loadWidgetState(_ui->widget_cloudViewer);
00448
00449
00450 _preferencesDialog->loadWidgetState(_exportDialog);
00451 _preferencesDialog->loadWidgetState(_postProcessingDialog);
00452
00453 if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
00454 {
00455 const std::map<std::string, float> & statistics = Statistics::defaultData();
00456 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
00457 {
00458 _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
00459 }
00460 }
00461 this->loadFigures();
00462 connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
00463
00464
00465 ParametersMap parameters = _preferencesDialog->getAllParameters();
00466 _ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
00467 _ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
00468 _ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
00469
00470
00471 _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
00472 _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
00473 _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
00474
00475 splash.close();
00476
00477 this->setFocus();
00478
00479 UDEBUG("");
00480 }
00481
00482 MainWindow::~MainWindow()
00483 {
00484 UDEBUG("");
00485 this->stopDetection();
00486 delete _ui;
00487 delete _elapsedTime;
00488 }
00489
00490 void MainWindow::setupMainLayout(bool vertical)
00491 {
00492 if(vertical)
00493 {
00494 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
00495 }
00496 else if(!vertical)
00497 {
00498 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
00499 }
00500 }
00501
00502 void MainWindow::closeEvent(QCloseEvent* event)
00503 {
00504
00505
00506
00507
00508
00509
00510
00511
00512 UDEBUG("");
00513 bool processStopped = true;
00514 if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
00515 {
00516 this->stopDetection();
00517 if(_state == kInitialized)
00518 {
00519 if(this->closeDatabase())
00520 {
00521 this->changeState(kApplicationClosing);
00522 }
00523 }
00524 if(_state != kIdle)
00525 {
00526 processStopped = false;
00527 }
00528 }
00529
00530 if(processStopped)
00531 {
00532
00533 bool save = false;
00534 if(this->isWindowModified())
00535 {
00536 QMessageBox::Button b=QMessageBox::question(this,
00537 tr("RTAB-Map"),
00538 tr("There are unsaved changed settings. Save them?"),
00539 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
00540 if(b == QMessageBox::Save)
00541 {
00542 save = true;
00543 }
00544 else if(b != QMessageBox::Discard)
00545 {
00546 event->ignore();
00547 return;
00548 }
00549 }
00550
00551 if(save)
00552 {
00553 saveConfigGUI();
00554 }
00555
00556 _ui->statsToolBox->closeFigures();
00557
00558 _ui->dockWidget_imageView->close();
00559 _ui->dockWidget_likelihood->close();
00560 _ui->dockWidget_rawlikelihood->close();
00561 _ui->dockWidget_posterior->close();
00562 _ui->dockWidget_statsV2->close();
00563 _ui->dockWidget_console->close();
00564 _ui->dockWidget_cloudViewer->close();
00565 _ui->dockWidget_loopClosureViewer->close();
00566 _ui->dockWidget_mapVisibility->close();
00567 _ui->dockWidget_graphViewer->close();
00568 _ui->dockWidget_odometry->close();
00569
00570 if(_camera)
00571 {
00572 UERROR("Camera must be already deleted here!");
00573 delete _camera;
00574 _camera = 0;
00575 }
00576 if(_dbReader)
00577 {
00578 UERROR("DBReader must be already deleted here!");
00579 delete _dbReader;
00580 _dbReader = 0;
00581 }
00582 if(_odomThread)
00583 {
00584 UERROR("OdomThread must be already deleted here!");
00585 delete _odomThread;
00586 _odomThread = 0;
00587 }
00588 event->accept();
00589 }
00590 else
00591 {
00592 event->ignore();
00593 }
00594 UDEBUG("");
00595 }
00596
00597 void MainWindow::handleEvent(UEvent* anEvent)
00598 {
00599 if(anEvent->getClassName().compare("RtabmapEvent") == 0)
00600 {
00601 RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
00602 Statistics stats = rtabmapEvent->getStats();
00603 int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
00604 int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_last_closure_id(), 0.0f));
00605 bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
00606 float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
00607 if((stats.loopClosureId() > 0 &&
00608 _ui->actionPause_on_match->isChecked())
00609 ||
00610 (stats.loopClosureId() == 0 &&
00611 highestHypothesisId > 0 &&
00612 highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
00613 _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
00614 rejectedHyp)
00615 ||
00616 (localLoopClosureId > 0 &&
00617 _ui->actionPause_on_local_loop_detection->isChecked()))
00618 {
00619 if(_state != kPaused && _state != kMonitoringPaused)
00620 {
00621 if(_preferencesDialog->beepOnPause())
00622 {
00623 QMetaObject::invokeMethod(this, "beep");
00624 }
00625 this->pauseDetection();
00626 }
00627 }
00628 _processingStatistics = true;
00629 emit statsReceived(stats);
00630 }
00631 else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
00632 {
00633 RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
00634 emit rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
00635 }
00636 else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
00637 {
00638 RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
00639 emit rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
00640 }
00641 else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
00642 {
00643 RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
00644 emit rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
00645 }
00646 else if(anEvent->getClassName().compare("CameraEvent") == 0)
00647 {
00648 CameraEvent * cameraEvent = (CameraEvent*)anEvent;
00649 if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
00650 {
00651 if(_preferencesDialog->beepOnPause())
00652 {
00653 QMetaObject::invokeMethod(this, "beep");
00654 }
00655 emit noMoreImagesReceived();
00656 }
00657 }
00658 else if(anEvent->getClassName().compare("OdometryEvent") == 0)
00659 {
00660
00661 if(UTimer::now() - _lastOdomInfoUpdateTime > 0.1)
00662 {
00663 _lastOdomInfoUpdateTime = UTimer::now();
00664 OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
00665 if(!_processingOdometry && !_processingStatistics)
00666 {
00667 _processingOdometry = true;
00668 emit odometryReceived(odomEvent->data(), odomEvent->info());
00669 }
00670 }
00671 }
00672 else if(anEvent->getClassName().compare("ULogEvent") == 0)
00673 {
00674 ULogEvent * logEvent = (ULogEvent*)anEvent;
00675 if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
00676 {
00677 QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
00678
00679 if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
00680 {
00681 _logEventTime->start();
00682 if(_preferencesDialog->beepOnPause())
00683 {
00684 QMetaObject::invokeMethod(this, "beep");
00685 }
00686 pauseDetection();
00687 }
00688 }
00689 }
00690 }
00691
00692 void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
00693 {
00694 _processingOdometry = true;
00695 UTimer time;
00696 Transform pose = data.pose();
00697 bool lost = false;
00698 bool lostStateChanged = false;
00699
00700 if(pose.isNull())
00701 {
00702 UDEBUG("odom lost");
00703 lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed;
00704 _ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
00705 _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
00706
00707 pose = _lastOdomPose;
00708 lost = true;
00709 }
00710 else if(info.inliers>0 &&
00711 _preferencesDialog->getOdomQualityWarnThr() &&
00712 info.inliers < _preferencesDialog->getOdomQualityWarnThr())
00713 {
00714 UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
00715 lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
00716 _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
00717 _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
00718 }
00719 else
00720 {
00721 UDEBUG("odom ok");
00722 lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
00723 _ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor());
00724 _ui->imageView_odometry->setBackgroundColor(Qt::black);
00725 }
00726
00727 if(info.inliers >= 0)
00728 {
00729 _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
00730 }
00731 if(info.matches >= 0)
00732 {
00733 _ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
00734 }
00735 if(info.variance >= 0)
00736 {
00737 _ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
00738 }
00739 if(info.variance >= 0)
00740 {
00741 _ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
00742 }
00743 if(info.time > 0)
00744 {
00745 _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f);
00746 }
00747 if(info.features >=0)
00748 {
00749 _ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
00750 }
00751 if(info.localMapSize >=0)
00752 {
00753 _ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
00754 }
00755 _ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
00756
00757 float x,y,z, roll,pitch,yaw;
00758 pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
00759 _ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
00760 _ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y);
00761 _ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z);
00762 _ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI);
00763 _ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
00764 _ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
00765
00766 if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
00767 {
00768 _lastOdomPose = pose;
00769 _odometryReceived = true;
00770 }
00771
00772 if(_ui->dockWidget_cloudViewer->isVisible())
00773 {
00774 if(!pose.isNull())
00775 {
00776
00777 if(data.depthOrRightImage().cols == data.image().cols &&
00778 data.depthOrRightImage().rows == data.image().rows &&
00779 !data.depthOrRightImage().empty() &&
00780 data.fx() > 0.0f &&
00781 data.fyOrBaseline() > 0.0f &&
00782 _preferencesDialog->isCloudsShown(1))
00783 {
00784 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00785 cloud = createCloud(0,
00786 data.image(),
00787 data.depthOrRightImage(),
00788 data.fx(),
00789 data.fyOrBaseline(),
00790 data.cx(),
00791 data.cy(),
00792 data.localTransform(),
00793 pose,
00794 _preferencesDialog->getCloudVoxelSize(1),
00795 _preferencesDialog->getCloudDecimation(1),
00796 _preferencesDialog->getCloudMaxDepth(1));
00797
00798 if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
00799 {
00800 UERROR("Adding cloudOdom to viewer failed!");
00801 }
00802 _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
00803 _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
00804 _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
00805 }
00806
00807
00808 if(!data.laserScan().empty() &&
00809 _preferencesDialog->isScansShown(1))
00810 {
00811 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
00812 cloud = util3d::laserScanToPointCloud(data.laserScan());
00813 cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
00814 if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
00815 {
00816 UERROR("Adding scanOdom to viewer failed!");
00817 }
00818 _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
00819 _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
00820 _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
00821 }
00822
00823 if(!data.pose().isNull())
00824 {
00825
00826 _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
00827 }
00828 }
00829 _ui->widget_cloudViewer->update();
00830 }
00831
00832 if(_ui->graphicsView_graphView->isVisible())
00833 {
00834 if(!pose.isNull() && !data.pose().isNull())
00835 {
00836 _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
00837 _ui->graphicsView_graphView->update();
00838 }
00839 }
00840
00841 if(_ui->dockWidget_odometry->isVisible() &&
00842 !data.image().empty())
00843 {
00844 if(_ui->imageView_odometry->isFeaturesShown())
00845 {
00846 if(info.type == 0)
00847 {
00848 _ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
00849 }
00850 else if(info.type == 1)
00851 {
00852 std::vector<cv::KeyPoint> kpts;
00853 cv::KeyPoint::convert(info.refCorners, kpts);
00854 _ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
00855 }
00856 }
00857
00858 _ui->imageView_odometry->clearLines();
00859 if(lost)
00860 {
00861 if(lostStateChanged)
00862 {
00863
00864 _odomImageShow = _ui->imageView_odometry->isImageShown();
00865 _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
00866 }
00867 _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
00868 _ui->imageView_odometry->setImageShown(true);
00869 _ui->imageView_odometry->setImageDepthShown(true);
00870 }
00871 else
00872 {
00873 if(lostStateChanged)
00874 {
00875
00876 _ui->imageView_odometry->setImageShown(_odomImageShow);
00877 _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
00878 }
00879
00880 _ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
00881 if(_ui->imageView_odometry->isImageDepthShown())
00882 {
00883 _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
00884 }
00885
00886 if(info.type == 0)
00887 {
00888 if(_ui->imageView_odometry->isFeaturesShown())
00889 {
00890 for(unsigned int i=0; i<info.wordMatches.size(); ++i)
00891 {
00892 _ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red);
00893 }
00894 for(unsigned int i=0; i<info.wordInliers.size(); ++i)
00895 {
00896 _ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green);
00897 }
00898 }
00899 }
00900 }
00901 if(info.type == 1 && info.cornerInliers.size())
00902 {
00903 if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
00904 {
00905
00906 UASSERT(info.refCorners.size() == info.newCorners.size());
00907 for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
00908 {
00909 if(_ui->imageView_odometry->isFeaturesShown())
00910 {
00911 _ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green);
00912 }
00913 if(_ui->imageView_odometry->isLinesShown())
00914 {
00915 _ui->imageView_odometry->addLine(
00916 info.refCorners[info.cornerInliers[i]].x,
00917 info.refCorners[info.cornerInliers[i]].y,
00918 info.newCorners[info.cornerInliers[i]].x,
00919 info.newCorners[info.cornerInliers[i]].y,
00920 Qt::blue);
00921 }
00922 }
00923 }
00924 }
00925 if(!data.image().empty())
00926 {
00927 _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
00928 }
00929
00930 _ui->imageView_odometry->update();
00931 }
00932
00933 if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
00934 {
00935 this->captureScreen();
00936 }
00937
00938 _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
00939
00940 _processingOdometry = false;
00941 }
00942
00943 void MainWindow::processStats(const rtabmap::Statistics & stat)
00944 {
00945 _processingStatistics = true;
00946 ULOGGER_DEBUG("");
00947 QTime time, totalTime;
00948 time.start();
00949 totalTime.start();
00950
00951
00952 int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
00953 int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
00954
00955 _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
00956 _ui->label_matchId->clear();
00957
00958 if(stat.extended())
00959 {
00960 float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
00961 if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
00962 {
00963 UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
00964 }
00965
00966 UDEBUG("");
00967 int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
00968 bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
00969
00970
00971 _ui->imageView_source->clear();
00972 _ui->imageView_loopClosure->clear();
00973 _ui->imageView_source->setBackgroundColor(Qt::black);
00974 _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
00975
00976
00977 Signature signature = stat.getSignature();
00978 signature.uncompressData();
00979 _cachedSignatures.insert(stat.getSignature().id(), signature);
00980
00981 int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
00982 int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
00983 bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
00984 _ui->label_matchId->clear();
00985 _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
00986
00987 if(rehearsed > 0)
00988 {
00989 _ui->imageView_source->setBackgroundColor(Qt::blue);
00990 }
00991 else if(localTimeClosures > 0)
00992 {
00993 _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
00994 }
00995 else if(scanMatchingSuccess)
00996 {
00997 _ui->imageView_source->setBackgroundColor(Qt::gray);
00998 }
00999
01000 UDEBUG("time= %d ms", time.restart());
01001
01002 int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
01003 float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
01004 int matchId = 0;
01005 Signature loopSignature;
01006 int shownLoopId = 0;
01007 if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
01008 {
01009 bool show = true;
01010 if(stat.loopClosureId() > 0)
01011 {
01012 _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
01013 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
01014 if(highestHypothesisIsSaved)
01015 {
01016 _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
01017 }
01018 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
01019 matchId = stat.loopClosureId();
01020 }
01021 else if(stat.localLoopClosureId())
01022 {
01023 _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
01024 _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.localLoopClosureId()).arg(loopMapId));
01025 matchId = stat.localLoopClosureId();
01026 }
01027 else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
01028 {
01029 show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
01030 if(show)
01031 {
01032 _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
01033 _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
01034 _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
01035 }
01036 }
01037 else
01038 {
01039 show = _preferencesDialog->imageHighestHypShown();
01040 if(show)
01041 {
01042 _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
01043 }
01044 }
01045
01046 if(show)
01047 {
01048 shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
01049 QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
01050 if(iter != _cachedSignatures.end())
01051 {
01052 iter.value().uncompressData();
01053 loopSignature = iter.value();
01054 }
01055 }
01056 }
01057 _refIds.push_back(stat.refImageId());
01058 _loopClosureIds.push_back(matchId);
01059
01060
01061 {
01062 UCvMat2QImageThread qimageThread(signature.getImageRaw());
01063 UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
01064 UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
01065 UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
01066 qimageThread.start();
01067 qdepthThread.start();
01068 qimageLoopThread.start();
01069 qdepthLoopThread.start();
01070 qimageThread.join();
01071 qdepthThread.join();
01072 qimageLoopThread.join();
01073 qdepthLoopThread.join();
01074 QImage img = qimageThread.getQImage();
01075 QImage lcImg = qimageLoopThread.getQImage();
01076 QImage depth = qdepthThread.getQImage();
01077 QImage lcDepth = qdepthLoopThread.getQImage();
01078 UDEBUG("time= %d ms", time.restart());
01079
01080 if(!img.isNull())
01081 {
01082 _ui->imageView_source->setImage(img);
01083 }
01084 if(!depth.isNull())
01085 {
01086 _ui->imageView_source->setImageDepth(depth);
01087 }
01088 if(!lcImg.isNull())
01089 {
01090 _ui->imageView_loopClosure->setImage(lcImg);
01091 }
01092 if(!lcDepth.isNull())
01093 {
01094 _ui->imageView_loopClosure->setImageDepth(lcDepth);
01095 }
01096 if(img.rect().isValid())
01097 {
01098 QRectF sceneRect = img.rect();
01099 _ui->imageView_source->setSceneRect(sceneRect);
01100 _ui->imageView_loopClosure->setSceneRect(sceneRect);
01101 }
01102 }
01103
01104 UDEBUG("time= %d ms", time.restart());
01105
01106
01107 this->drawKeypoints(signature.getWords(), loopSignature.getWords());
01108
01109 UDEBUG("time= %d ms", time.restart());
01110
01111 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), signature.getWords().size());
01112 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), loopSignature.getWords().size());
01113
01114
01115 if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
01116 {
01117 UDEBUG("");
01118 if(stat.weights().size() != stat.posterior().size())
01119 {
01120 UWARN("%d %d", stat.weights().size(), stat.posterior().size());
01121 }
01122 _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
01123
01124 ULOGGER_DEBUG("");
01125
01126 float value;
01127 value = float(_preferencesDialog->getLoopThr());
01128 emit(loopClosureThrChanged(value));
01129 }
01130 if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
01131 {
01132 _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
01133 }
01134 if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
01135 {
01136 _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
01137 }
01138
01139
01140 const std::map<std::string, float> & statistics = stat.data();
01141 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
01142 {
01143
01144 _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), stat.refImageId(), (*iter).second);
01145 }
01146
01147 UDEBUG("time= %d ms", time.restart());
01148
01149
01150
01151
01152 UTimer timerVis;
01153
01154
01155 if(stat.poses().size())
01156 {
01157
01158 updateMapCloud(stat.poses(),
01159 _odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
01160 stat.constraints(),
01161 stat.getMapIds());
01162
01163 _odometryReceived = false;
01164
01165 _odometryCorrection = stat.mapCorrection();
01166
01167 UDEBUG("time= %d ms", time.restart());
01168 _ui->statsToolBox->updateStat("/Gui RGB-D cloud/ms", stat.refImageId(), int(timerVis.elapsed()*1000.0f));
01169
01170
01171 if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
01172 !stat.loopClosureTransform().isNull() &&
01173 !loopSignature.getImageRaw().empty())
01174 {
01175
01176 Transform loopClosureTransform = stat.loopClosureTransform();
01177 signature.setPose(loopClosureTransform);
01178 _ui->widget_loopClosureViewer->setData(loopSignature, signature);
01179 if(_ui->dockWidget_loopClosureViewer->isVisible())
01180 {
01181 UTimer loopTimer;
01182 _ui->widget_loopClosureViewer->updateView();
01183 UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
01184 _ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
01185 }
01186
01187 UDEBUG("time= %d ms", time.restart());
01188 }
01189 }
01190
01191
01192 if(_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible() && stat.posterior().size())
01193 {
01194 _ui->graphicsView_graphView->updatePosterior(stat.posterior());
01195 }
01196
01197 _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
01198 if(stat.localPath().size() == 0)
01199 {
01200
01201 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
01202 }
01203
01204 if(stat.currentGoalId() > 0)
01205 {
01206 _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId());
01207 }
01208
01209 UDEBUG("");
01210 }
01211 else if(!stat.extended() && stat.loopClosureId()>0)
01212 {
01213 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
01214 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
01215 }
01216 float elapsedTime = static_cast<float>(totalTime.elapsed());
01217 UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
01218 _ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
01219 if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
01220 {
01221 this->captureScreen();
01222 }
01223
01224 if(!_preferencesDialog->isImagesKept())
01225 {
01226 _cachedSignatures.clear();
01227 }
01228
01229 _processingStatistics = false;
01230 }
01231
01232 void MainWindow::updateMapCloud(
01233 const std::map<int, Transform> & posesIn,
01234 const Transform & currentPose,
01235 const std::multimap<int, Link> & constraints,
01236 const std::map<int, int> & mapIdsIn,
01237 bool verboseProgress)
01238 {
01239 UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d currentPose=%s",
01240 (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), currentPose.prettyPrint().c_str());
01241 if(posesIn.size())
01242 {
01243 _currentPosesMap = posesIn;
01244 _currentLinksMap = constraints;
01245 _currentMapIds = mapIdsIn;
01246 if(_currentPosesMap.size())
01247 {
01248 if(!_ui->actionSave_point_cloud->isEnabled() &&
01249 _cachedSignatures.size() &&
01250 (!(--_cachedSignatures.end())->getDepthCompressed().empty() ||
01251 !(--_cachedSignatures.end())->getWords3().empty()))
01252 {
01253
01254 _ui->actionSave_point_cloud->setEnabled(true);
01255 _ui->actionView_high_res_point_cloud->setEnabled(true);
01256 }
01257
01258 if(!_ui->actionView_scans->isEnabled() &&
01259 _cachedSignatures.size() &&
01260 !(--_cachedSignatures.end())->getLaserScanCompressed().empty())
01261 {
01262 _ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
01263 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
01264 _ui->actionView_scans->setEnabled(true);
01265 }
01266 else if(_preferencesDialog->isGridMapFrom3DCloud() && _projectionLocalMaps.size())
01267 {
01268 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
01269 }
01270 }
01271 if(_state != kMonitoring && _state != kDetecting)
01272 {
01273 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
01274 }
01275 }
01276
01277
01278 std::map<int, Transform> poses;
01279 std::map<int, int> mapIds;
01280 if(_preferencesDialog->isCloudFiltering() && posesIn.size())
01281 {
01282 float radius = _preferencesDialog->getCloudFilteringRadius();
01283 float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0;
01284 poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
01285
01286 poses.insert(*posesIn.rbegin());
01287 for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
01288 {
01289 std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
01290 if(jter!=mapIdsIn.end())
01291 {
01292 mapIds.insert(*jter);
01293 }
01294 else
01295 {
01296 UERROR("map id of node %d not found!", iter->first);
01297 }
01298
01299 }
01300 }
01301 else
01302 {
01303 poses = posesIn;
01304 mapIds = mapIdsIn;
01305 }
01306
01307 std::map<int, bool> posesMask;
01308 for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
01309 {
01310 posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
01311 }
01312 _ui->widget_mapVisibility->setMap(posesIn, posesMask);
01313
01314
01315 UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
01316 QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
01317 int i=1;
01318 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
01319 {
01320 if(!iter->second.isNull())
01321 {
01322 std::string cloudName = uFormat("cloud%d", iter->first);
01323
01324
01325 if((_ui->widget_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
01326 (_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud()))
01327 {
01328 if(viewerClouds.contains(cloudName))
01329 {
01330
01331 Transform tCloud;
01332 _ui->widget_cloudViewer->getPose(cloudName, tCloud);
01333 if(tCloud.isNull() || iter->second != tCloud)
01334 {
01335 if(!_ui->widget_cloudViewer->updateCloudPose(cloudName, iter->second))
01336 {
01337 UERROR("Updating pose cloud %d failed!", iter->first);
01338 }
01339 }
01340 _ui->widget_cloudViewer->setCloudVisibility(cloudName, true);
01341 _ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
01342 _ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
01343 }
01344 else if(_cachedSignatures.contains(iter->first))
01345 {
01346 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
01347 if((!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty()) || jter->getWords3().size())
01348 {
01349 this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
01350 }
01351 }
01352 }
01353 else if(viewerClouds.contains(cloudName))
01354 {
01355 UDEBUG("Hide cloud %s", cloudName.c_str());
01356 _ui->widget_cloudViewer->setCloudVisibility(cloudName.c_str(), false);
01357 }
01358
01359
01360 std::string scanName = uFormat("scan%d", iter->first);
01361 if((_ui->widget_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
01362 (_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
01363 {
01364 if(viewerClouds.contains(scanName))
01365 {
01366
01367 Transform tScan;
01368 _ui->widget_cloudViewer->getPose(scanName, tScan);
01369 if(tScan.isNull() || iter->second != tScan)
01370 {
01371 if(!_ui->widget_cloudViewer->updateCloudPose(scanName, iter->second))
01372 {
01373 UERROR("Updating pose scan %d failed!", iter->first);
01374 }
01375 }
01376 _ui->widget_cloudViewer->setCloudVisibility(scanName, true);
01377 _ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
01378 _ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
01379 }
01380 else if(_cachedSignatures.contains(iter->first))
01381 {
01382 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
01383 if(!jter->getLaserScanCompressed().empty())
01384 {
01385 this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
01386 }
01387 }
01388 if(!_preferencesDialog->isScansShown(0))
01389 {
01390 UDEBUG("Hide scan %s", scanName.c_str());
01391 _ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
01392 }
01393 }
01394 else if(viewerClouds.contains(scanName))
01395 {
01396 UDEBUG("Hide scan %s", scanName.c_str());
01397 _ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
01398 }
01399
01400 if(verboseProgress)
01401 {
01402 _initProgressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
01403 _initProgressDialog->incrementStep();
01404 QApplication::processEvents();
01405 }
01406 }
01407
01408 ++i;
01409 }
01410
01411
01412 for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
01413 {
01414 std::list<std::string> splitted = uSplitNumChar(iter.key());
01415 if(splitted.size() == 2)
01416 {
01417 int id = std::atoi(splitted.back().c_str());
01418 if(poses.find(id) == poses.end())
01419 {
01420 if(_ui->widget_cloudViewer->getCloudVisibility(iter.key()))
01421 {
01422 UDEBUG("Hide %s", iter.key().c_str());
01423 _ui->widget_cloudViewer->setCloudVisibility(iter.key(), false);
01424 }
01425 }
01426 }
01427 }
01428
01429
01430 _ui->widget_cloudViewer->removeAllGraphs();
01431 _ui->widget_cloudViewer->removeCloud("graph_nodes");
01432 if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
01433 {
01434
01435 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
01436 for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
01437 {
01438 int mapId = uValue(_currentMapIds, iter->first, -1);
01439
01440
01441 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
01442 if(kter == graphs.end())
01443 {
01444 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
01445 }
01446 pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
01447 kter->second->push_back(pt);
01448 }
01449
01450
01451 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
01452 {
01453 QColor color = Qt::gray;
01454 if(iter->first >= 0)
01455 {
01456 color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
01457 }
01458 _ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
01459 }
01460 }
01461
01462
01463 if(_ui->graphicsView_graphView->isVisible())
01464 {
01465 _ui->graphicsView_graphView->updateGraph(posesIn, constraints);
01466 if(!currentPose.isNull())
01467 {
01468 _ui->graphicsView_graphView->updateReferentialPosition(currentPose);
01469 }
01470 }
01471 cv::Mat map8U;
01472 if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
01473 {
01474 float xMin, yMin;
01475 float resolution = _preferencesDialog->getGridMapResolution();
01476 cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
01477 poses,
01478 _preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
01479 resolution,
01480 xMin, yMin,
01481 0,
01482 _preferencesDialog->isGridMapEroded());
01483 if(!map8S.empty())
01484 {
01485
01486 map8U = util3d::convertMap2Image8U(map8S);
01487
01488 if(_preferencesDialog->getGridMapShown())
01489 {
01490 float opacity = _preferencesDialog->getGridMapOpacity();
01491 _ui->widget_cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
01492 }
01493 if(_ui->graphicsView_graphView->isVisible())
01494 {
01495 _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
01496 }
01497 }
01498 }
01499 _ui->graphicsView_graphView->update();
01500
01501 if(!_preferencesDialog->getGridMapShown())
01502 {
01503 _ui->widget_cloudViewer->removeOccupancyGridMap();
01504 }
01505
01506 if(viewerClouds.contains("cloudOdom"))
01507 {
01508 if(!_preferencesDialog->isCloudsShown(1))
01509 {
01510 _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
01511 }
01512 else
01513 {
01514 _ui->widget_cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
01515 _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
01516 _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
01517 }
01518 }
01519 if(viewerClouds.contains("scanOdom"))
01520 {
01521 if(!_preferencesDialog->isScansShown(1))
01522 {
01523 _ui->widget_cloudViewer->setCloudVisibility("scanOdom", false);
01524 }
01525 else
01526 {
01527 _ui->widget_cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
01528 _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
01529 _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
01530 }
01531 }
01532
01533 if(!currentPose.isNull())
01534 {
01535 _ui->widget_cloudViewer->updateCameraTargetPosition(currentPose);
01536 }
01537
01538 _ui->widget_cloudViewer->update();
01539 }
01540
01541 void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
01542 {
01543 UASSERT(!pose.isNull());
01544 std::string cloudName = uFormat("cloud%d", nodeId);
01545 if(_ui->widget_cloudViewer->getAddedClouds().contains(cloudName))
01546 {
01547 UERROR("Cloud %d already added to map.", nodeId);
01548 return;
01549 }
01550
01551 QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
01552 if(iter == _cachedSignatures.end())
01553 {
01554 UERROR("Node %d is not in the cache.", nodeId);
01555 return;
01556 }
01557
01558 if(!iter->getImageCompressed().empty() && !iter->getDepthCompressed().empty())
01559 {
01560
01561 cv::Mat image, depth;
01562 iter->uncompressData(&image, &depth, 0);
01563
01564 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01565 cloud = createCloud(nodeId,
01566 image,
01567 depth,
01568 iter->getFx(),
01569 iter->getFy(),
01570 iter->getCx(),
01571 iter->getCy(),
01572 iter->getLocalTransform(),
01573 Transform::getIdentity(),
01574 _preferencesDialog->getCloudVoxelSize(0),
01575 _preferencesDialog->getCloudDecimation(0),
01576 _preferencesDialog->getCloudMaxDepth(0));
01577
01578 if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
01579 {
01580 UTimer timer;
01581 float cellSize = _preferencesDialog->getGridMapResolution();
01582 float groundNormalMaxAngle = M_PI_4;
01583 int minClusterSize = 20;
01584 cv::Mat ground, obstacles;
01585 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = cloud;
01586 if(voxelizedCloud->size())
01587 {
01588 voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
01589 }
01590 util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
01591 voxelizedCloud,
01592 ground, obstacles,
01593 cellSize,
01594 groundNormalMaxAngle,
01595 minClusterSize);
01596 if(!ground.empty() || !obstacles.empty())
01597 {
01598 _projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
01599 }
01600 UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
01601 }
01602
01603 if(_preferencesDialog->isCloudMeshing())
01604 {
01605 pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
01606 if(cloud->size())
01607 {
01608 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
01609 if(_preferencesDialog->getMeshSmoothing())
01610 {
01611 cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius());
01612 }
01613 else
01614 {
01615 cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch());
01616 }
01617 mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius());
01618 }
01619
01620 if(mesh->polygons.size())
01621 {
01622 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
01623 pcl::fromPCLPointCloud2(mesh->cloud, *tmp);
01624 if(!_ui->widget_cloudViewer->addCloudMesh(cloudName, tmp, mesh->polygons, pose))
01625 {
01626 UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
01627 }
01628 else
01629 {
01630 _createdClouds.insert(std::make_pair(nodeId, tmp));
01631 }
01632 }
01633 }
01634 else
01635 {
01636 if(_preferencesDialog->getMeshSmoothing())
01637 {
01638 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
01639 cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius());
01640 cloud->clear();
01641 pcl::copyPointCloud(*cloudWithNormals, *cloud);
01642 }
01643 QColor color = Qt::gray;
01644 if(mapId >= 0)
01645 {
01646 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
01647 }
01648 if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
01649 {
01650 UERROR("Adding cloud %d to viewer failed!", nodeId);
01651 }
01652 else
01653 {
01654 _createdClouds.insert(std::make_pair(nodeId, cloud));
01655 }
01656 }
01657 }
01658 else if(iter->getWords3().size())
01659 {
01660 QColor color = Qt::gray;
01661 if(mapId >= 0)
01662 {
01663 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
01664 }
01665 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01666 cloud->resize(iter->getWords3().size());
01667 int oi=0;
01668 for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->getWords3().begin(); jter!=iter->getWords3().end(); ++jter)
01669 {
01670 (*cloud)[oi].x = jter->second.x;
01671 (*cloud)[oi].y = jter->second.y;
01672 (*cloud)[oi].z = jter->second.z;
01673 (*cloud)[oi].r = 255;
01674 (*cloud)[oi].g = 255;
01675 (*cloud)[oi++].b = 255;
01676 }
01677 if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
01678 {
01679 UERROR("Adding cloud %d to viewer failed!", nodeId);
01680 }
01681 else
01682 {
01683 _createdClouds.insert(std::make_pair(nodeId, cloud));
01684 }
01685 }
01686 else
01687 {
01688 return;
01689 }
01690
01691 _ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
01692 _ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
01693 }
01694
01695 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
01696 {
01697 std::string scanName = uFormat("scan%d", nodeId);
01698 if(_ui->widget_cloudViewer->getAddedClouds().contains(scanName))
01699 {
01700 UERROR("Scan %d already added to map.", nodeId);
01701 return;
01702 }
01703
01704 QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
01705 if(iter == _cachedSignatures.end())
01706 {
01707 UERROR("Node %d is not in the cache.", nodeId);
01708 return;
01709 }
01710
01711 if(!iter->getLaserScanCompressed().empty())
01712 {
01713 cv::Mat depth2D;
01714 iter->uncompressData(0, 0, &depth2D);
01715
01716 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
01717 cloud = util3d::laserScanToPointCloud(depth2D);
01718 QColor color = Qt::gray;
01719 if(mapId >= 0)
01720 {
01721 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
01722 }
01723 if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
01724 {
01725 UERROR("Adding cloud %d to viewer failed!", nodeId);
01726 }
01727 else
01728 {
01729 _createdScans.insert(std::make_pair(nodeId, cloud));
01730
01731 cv::Mat ground, obstacles;
01732 util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
01733 _gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
01734 }
01735 _ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
01736 _ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
01737 }
01738 }
01739
01740 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
01741 {
01742 if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
01743 {
01744 QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
01745 if(_preferencesDialog->isCloudsShown(0))
01746 {
01747 std::string cloudName = uFormat("cloud%d", nodeId);
01748 if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
01749 {
01750 createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
01751 }
01752 else if(viewerClouds.contains(cloudName))
01753 {
01754 if(visible)
01755 {
01756
01757 _ui->widget_cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second);
01758 }
01759 _ui->widget_cloudViewer->setCloudVisibility(cloudName, visible);
01760 }
01761 }
01762
01763 if(_preferencesDialog->isScansShown(0))
01764 {
01765 std::string scanName = uFormat("scan%d", nodeId);
01766 if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
01767 {
01768 createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
01769 }
01770 else if(viewerClouds.contains(scanName))
01771 {
01772 if(visible)
01773 {
01774
01775 _ui->widget_cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second);
01776 }
01777 _ui->widget_cloudViewer->setCloudVisibility(scanName, visible);
01778 }
01779 }
01780 }
01781 _ui->widget_cloudViewer->update();
01782 }
01783
01784 void MainWindow::processRtabmapEventInit(int status, const QString & info)
01785 {
01786 if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
01787 {
01788 _initProgressDialog->setAutoClose(true, 1);
01789 _initProgressDialog->resetProgress();
01790 _initProgressDialog->show();
01791 this->changeState(MainWindow::kInitializing);
01792 }
01793 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
01794 {
01795 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
01796 this->changeState(MainWindow::kInitialized);
01797 }
01798 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
01799 {
01800 _initProgressDialog->setAutoClose(true, 1);
01801 _initProgressDialog->resetProgress();
01802 _initProgressDialog->show();
01803 if(_state!=kApplicationClosing)
01804 {
01805 this->changeState(MainWindow::kClosing);
01806 }
01807 }
01808 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
01809 {
01810 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
01811
01812 if(_databaseUpdated)
01813 {
01814 if(!_newDatabasePath.isEmpty())
01815 {
01816 if(!_newDatabasePathOutput.isEmpty())
01817 {
01818 if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
01819 {
01820 std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
01821 UINFO(msg.c_str());
01822 QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
01823 }
01824 else
01825 {
01826 std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".", _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
01827 UERROR(msg.c_str());
01828 QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
01829 }
01830 }
01831 else if(QFile::remove(_newDatabasePath))
01832 {
01833 UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
01834 }
01835 else
01836 {
01837 UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
01838 }
01839
01840 }
01841 else if(!_openedDatabasePath.isEmpty())
01842 {
01843 std::string msg = uFormat("Database \"%s\" updated.", _openedDatabasePath.toStdString().c_str());
01844 UINFO(msg.c_str());
01845 QMessageBox::information(this, tr("Database updated!"), QString(msg.c_str()));
01846 }
01847 }
01848 else if(!_newDatabasePath.isEmpty())
01849 {
01850
01851 if(QFile::remove(_newDatabasePath))
01852 {
01853 UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
01854 }
01855 else
01856 {
01857 UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
01858 }
01859 }
01860 _openedDatabasePath.clear();
01861 _newDatabasePath.clear();
01862 _newDatabasePathOutput.clear();
01863 bool applicationClosing = _state == kApplicationClosing;
01864 this->changeState(MainWindow::kIdle);
01865 if(applicationClosing)
01866 {
01867 this->close();
01868 }
01869 }
01870 else
01871 {
01872 _initProgressDialog->incrementStep();
01873 QString msg(info);
01874 if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
01875 {
01876 _openedDatabasePath.clear();
01877 _newDatabasePath.clear();
01878 _newDatabasePathOutput.clear();
01879 _initProgressDialog->setAutoClose(false);
01880 msg.prepend(tr("[ERROR] "));
01881 _initProgressDialog->appendText(msg);
01882 this->changeState(MainWindow::kIdle);
01883 }
01884 else
01885 {
01886 _initProgressDialog->appendText(msg);
01887 }
01888 }
01889 }
01890
01891 void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event)
01892 {
01893 _initProgressDialog->appendText("Downloading the map... done.");
01894 _initProgressDialog->incrementStep();
01895
01896 if(event.getCode())
01897 {
01898 UERROR("Map received with code error %d!", event.getCode());
01899 _initProgressDialog->appendText(uFormat("[ERROR] Map received with code error %d!", event.getCode()).c_str());
01900 _initProgressDialog->setAutoClose(false);
01901 }
01902 else
01903 {
01904
01905 UINFO("Received map!");
01906 UINFO(" signatures = %d", event.getSignatures().size());
01907 UINFO(" map ids = %d", event.getMapIds().size());
01908 UINFO(" poses = %d", event.getPoses().size());
01909 UINFO(" constraints = %d", event.getConstraints().size());
01910
01911 _initProgressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
01912 _initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
01913 QApplication::processEvents();
01914
01915 int addedSignatures = 0;
01916 for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
01917 iter!=event.getSignatures().end();
01918 ++iter)
01919 {
01920 if(!_cachedSignatures.contains(iter->first))
01921 {
01922 _cachedSignatures.insert(iter->first, iter->second);
01923 ++addedSignatures;
01924 }
01925 _initProgressDialog->incrementStep();
01926 QApplication::processEvents();
01927 }
01928 _initProgressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
01929 _initProgressDialog->incrementStep();
01930 QApplication::processEvents();
01931
01932 _initProgressDialog->appendText("Inserting data in the cache... done.");
01933
01934 if(event.getPoses().size())
01935 {
01936 _initProgressDialog->appendText("Updating the 3D map cloud...");
01937 _initProgressDialog->incrementStep();
01938 QApplication::processEvents();
01939 this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), event.getMapIds(), true);
01940 _initProgressDialog->appendText("Updating the 3D map cloud... done.");
01941 }
01942 else
01943 {
01944 _initProgressDialog->appendText("No poses received! The map cloud cannot be updated...");
01945 UINFO("Map received is empty! Cannot update the map cloud...");
01946 }
01947
01948 _initProgressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
01949
01950 if(!_preferencesDialog->isImagesKept())
01951 {
01952 _cachedSignatures.clear();
01953 }
01954 }
01955 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
01956 }
01957
01958 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
01959 {
01960 if(!event.getPoses().empty())
01961 {
01962 _ui->graphicsView_graphView->setGlobalPath(event.getPoses());
01963 }
01964
01965 if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
01966 {
01967
01968 if(event.getPoses().empty())
01969 {
01970 QMessageBox * warn = new QMessageBox(
01971 QMessageBox::Warning,
01972 tr("Setting goal failed!"),
01973 tr("Setting goal to location %1 failed. "
01974 "Some reasons: \n"
01975 "1) the location doesn't exist in the graph,\n"
01976 "2) the location is not linked to the global graph or\n"
01977 "3) the location is too near of the current location (goal already reached).").arg(event.getGoal()),
01978 QMessageBox::Ok,
01979 this);
01980 warn->setAttribute(Qt::WA_DeleteOnClose, true);
01981 warn->show();
01982 }
01983 else
01984 {
01985 QMessageBox * info = new QMessageBox(
01986 QMessageBox::Information,
01987 tr("Goal detected!"),
01988 tr("Global path computed from %1 to %2 (%3 poses, %4 m).").arg(event.getPoses().front().first).arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
01989 QMessageBox::Ok,
01990 this);
01991 info->setAttribute(Qt::WA_DeleteOnClose, true);
01992 info->show();
01993 }
01994 }
01995 }
01996
01997 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
01998 {
01999 ULOGGER_DEBUG("");
02000 if(flags & PreferencesDialog::kPanelSource)
02001 {
02002
02003 _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
02004 this->updateSelectSourceMenu();
02005 QString src;
02006 if(_preferencesDialog->isSourceImageUsed())
02007 {
02008 src = _preferencesDialog->getSourceImageTypeStr();
02009 }
02010 else if(_preferencesDialog->isSourceDatabaseUsed())
02011 {
02012 src = "Database";
02013 }
02014 _ui->label_stats_source->setText(src);
02015
02016 if(_camera)
02017 {
02018 _camera->setImageRate(_preferencesDialog->getGeneralInputRate());
02019
02020 if(_camera->cameraRGBD() && dynamic_cast<CameraOpenNI2*>(_camera->cameraRGBD()) != 0)
02021 {
02022 ((CameraOpenNI2*)_camera->cameraRGBD())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
02023 ((CameraOpenNI2*)_camera->cameraRGBD())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
02024 if(CameraOpenNI2::exposureGainAvailable())
02025 {
02026 ((CameraOpenNI2*)_camera->cameraRGBD())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
02027 ((CameraOpenNI2*)_camera->cameraRGBD())->setGain(_preferencesDialog->getSourceOpenni2Gain());
02028 }
02029 }
02030 if(_camera->camera())
02031 {
02032 _camera->camera()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
02033 }
02034 if(_camera->cameraRGBD())
02035 {
02036 _camera->cameraRGBD()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
02037 _camera->cameraRGBD()->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
02038 }
02039 }
02040 if(_dbReader)
02041 {
02042 _dbReader->setFrameRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
02043 }
02044 }
02045
02046 if(flags & PreferencesDialog::kPanelGeneral)
02047 {
02048 UDEBUG("General settings changed...");
02049 setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
02050 if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
02051 {
02052 _ui->graphicsView_graphView->clearPosterior();
02053 }
02054 }
02055
02056 if(flags & PreferencesDialog::kPanelCloudRendering)
02057 {
02058 UDEBUG("Cloud rendering settings changed...");
02059 if(_currentPosesMap.size())
02060 {
02061 this->updateMapCloud(
02062 std::map<int, Transform>(_currentPosesMap),
02063 Transform(),
02064 std::multimap<int, Link>(_currentLinksMap),
02065 std::map<int, int>(_currentMapIds));
02066 }
02067 }
02068
02069 if(flags & PreferencesDialog::kPanelLogging)
02070 {
02071 UDEBUG("Logging settings changed...");
02072 ULogger::setLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerLevel());
02073 ULogger::setEventLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerEventLevel());
02074 ULogger::setType((ULogger::Type)_preferencesDialog->getGeneralLoggerType(),
02075 (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
02076 ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
02077 }
02078 }
02079
02080 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
02081 {
02082 applyPrefSettings(parameters, true);
02083 }
02084
02085 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
02086 {
02087 ULOGGER_DEBUG("");
02088 if(parameters.size())
02089 {
02090 for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
02091 {
02092 UDEBUG("Parameter changed: Key=%s Value=%s", iter->first.c_str(), iter->second.c_str());
02093 }
02094
02095 rtabmap::ParametersMap parametersModified = parameters;
02096
02097 if(_state != kIdle && parametersModified.size())
02098 {
02099 if(parametersModified.erase(Parameters::kRtabmapWorkingDirectory()))
02100 {
02101 if(_state == kMonitoring || _state == kMonitoringPaused)
02102 {
02103 QMessageBox::information(this, tr("Working memory changed"), tr("The remote working directory can't be changed while the interface is in monitoring mode."));
02104 }
02105 else
02106 {
02107 QMessageBox::information(this, tr("Working memory changed"), tr("The working directory can't be changed while the detector is running. This will be applied when the detector will stop."));
02108 }
02109 }
02110 if(postParamEvent)
02111 {
02112 this->post(new ParamEvent(parametersModified));
02113 }
02114 }
02115
02116 if(_state != kMonitoring && _state != kMonitoringPaused &&
02117 uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
02118 {
02119 _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
02120 _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
02121 _ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
02122 }
02123
02124
02125 if(uContains(parameters, Parameters::kLccIcp3Decimation()))
02126 {
02127 _ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
02128 }
02129 if(uContains(parameters, Parameters::kLccIcp3MaxDepth()))
02130 {
02131 _ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
02132 }
02133 if(uContains(parameters, Parameters::kLccIcp3Samples()))
02134 {
02135 _ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
02136 }
02137
02138
02139 if(uContains(parameters, Parameters::kRGBDLocalRadius()))
02140 {
02141 _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
02142 }
02143 }
02144
02145
02146 _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
02147 _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
02148 _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
02149
02150 float value;
02151 value = float(_preferencesDialog->getLoopThr());
02152 emit(loopClosureThrChanged(value));
02153 }
02154
02155 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
02156 {
02157 UTimer timer;
02158
02159 timer.start();
02160 ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
02161 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
02162 {
02163 int id = iter->first;
02164 QColor color;
02165 if(uContains(loopWords, id))
02166 {
02167
02168 color = Qt::magenta;
02169 }
02170 else if(_lastIds.contains(id))
02171 {
02172
02173 color = Qt::blue;
02174 }
02175 else if(id<=_lastId)
02176 {
02177
02178 color = Qt::red;
02179 }
02180 else if(refWords.count(id) > 1)
02181 {
02182
02183 color = Qt::yellow;
02184 }
02185 else
02186 {
02187
02188 color = Qt::green;
02189 }
02190 _ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
02191 }
02192 ULOGGER_DEBUG("source time = %f s", timer.ticks());
02193
02194 timer.start();
02195 ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
02196 QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
02197 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
02198 {
02199 int id = iter->first;
02200 QColor color;
02201 if(uContains(refWords, id))
02202 {
02203
02204 color = Qt::magenta;
02205
02206 if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
02207 {
02208 const cv::KeyPoint & a = refWords.find(id)->second;
02209 const cv::KeyPoint & b = iter->second;
02210 uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
02211 }
02212 }
02213 else if(id<=_lastId)
02214 {
02215
02216 color = Qt::red;
02217 }
02218 else if(refWords.count(id) > 1)
02219 {
02220
02221 color = Qt::yellow;
02222 }
02223 else
02224 {
02225
02226 color = Qt::green;
02227 }
02228 _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
02229 }
02230
02231 ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
02232
02233 if(refWords.size()>0)
02234 {
02235 if((*refWords.rbegin()).first > _lastId)
02236 {
02237 _lastId = (*refWords.rbegin()).first;
02238 }
02239 _lastIds = QSet<int>::fromList(QList<int>::fromStdList(uKeysList(refWords)));
02240 }
02241
02242
02243 float scale = _ui->imageView_source->viewScale();
02244 UDEBUG("scale=%f", scale);
02245 float deltaX = _ui->imageView_source->width()/scale;
02246 float deltaY = 0;
02247 if(_preferencesDialog->isVerticalLayoutUsed())
02248 {
02249 deltaX = 0;
02250 deltaY = _ui->imageView_source->height()/scale;
02251 deltaY += _ui->label_matchId->height()/scale;
02252 }
02253 for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
02254 iter!=uniqueCorrespondences.end();
02255 ++iter)
02256 {
02257 _ui->imageView_source->addLine(
02258 iter->first.x,
02259 iter->first.y,
02260 iter->second.x+deltaX,
02261 iter->second.y+deltaY,
02262 Qt::cyan);
02263
02264 _ui->imageView_loopClosure->addLine(
02265 iter->first.x-deltaX,
02266 iter->first.y-deltaY,
02267 iter->second.x,
02268 iter->second.y,
02269 Qt::cyan);
02270 }
02271 _ui->imageView_source->update();
02272 _ui->imageView_loopClosure->update();
02273 }
02274
02275 void MainWindow::showEvent(QShowEvent* anEvent)
02276 {
02277
02278 this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
02279 }
02280
02281 void MainWindow::moveEvent(QMoveEvent* anEvent)
02282 {
02283 if(this->isVisible())
02284 {
02285
02286 if(!_firstCall)
02287 {
02288 this->configGUIModified();
02289 }
02290 _firstCall = false;
02291 }
02292 }
02293
02294 void MainWindow::resizeEvent(QResizeEvent* anEvent)
02295 {
02296 if(this->isVisible())
02297 {
02298 this->configGUIModified();
02299 }
02300 }
02301
02302 bool MainWindow::eventFilter(QObject *obj, QEvent *event)
02303 {
02304 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
02305 {
02306 this->setWindowModified(true);
02307 }
02308 return QWidget::eventFilter(obj, event);
02309 }
02310
02311 void MainWindow::updateSelectSourceMenu()
02312 {
02313 _ui->actionUsbCamera->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcUsbDevice);
02314 _ui->actionImageFiles->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages);
02315 _ui->actionVideo->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo);
02316
02317 _ui->actionDatabase->setChecked(_preferencesDialog->isSourceDatabaseUsed());
02318
02319 _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
02320 _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
02321 _ui->actionFreenect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect);
02322 _ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV);
02323 _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
02324 _ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
02325 _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
02326 _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
02327 _ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2);
02328 _ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394);
02329 _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoFlyCapture2);
02330 }
02331
02332 void MainWindow::changeImgRateSetting()
02333 {
02334 emit imgRateChanged(_ui->doubleSpinBox_stats_imgRate->value());
02335 }
02336
02337 void MainWindow::changeDetectionRateSetting()
02338 {
02339 emit detectionRateChanged(_ui->doubleSpinBox_stats_detectionRate->value());
02340 }
02341
02342 void MainWindow::changeTimeLimitSetting()
02343 {
02344 emit timeLimitChanged((float)_ui->doubleSpinBox_stats_timeLimit->value());
02345 }
02346
02347 void MainWindow::changeMappingMode()
02348 {
02349 emit mappingModeChanged(_ui->actionSLAM_mode->isChecked());
02350 }
02351
02352 void MainWindow::captureScreen()
02353 {
02354 QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
02355 QDir dir;
02356 if(!dir.exists(targetDir))
02357 {
02358 dir.mkdir(targetDir);
02359 }
02360 targetDir += QDir::separator();
02361 targetDir += "Main_window";
02362 if(!dir.exists(targetDir))
02363 {
02364 dir.mkdir(targetDir);
02365 }
02366 targetDir += QDir::separator();
02367 QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png");
02368 _ui->statusbar->clearMessage();
02369 QPixmap figure = QPixmap::grabWidget(this);
02370 figure.save(targetDir + name);
02371 QString msg = tr("Screen captured \"%1\"").arg(targetDir + name);
02372 _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
02373 _ui->widget_console->appendMsg(msg);
02374 }
02375
02376 void MainWindow::beep()
02377 {
02378 QApplication::beep();
02379 }
02380
02381 void MainWindow::configGUIModified()
02382 {
02383 this->setWindowModified(true);
02384 }
02385
02386
02387 void MainWindow::saveConfigGUI()
02388 {
02389 _savedMaximized = this->isMaximized();
02390 _preferencesDialog->saveMainWindowState(this);
02391 _preferencesDialog->saveWindowGeometry(_preferencesDialog);
02392 _preferencesDialog->saveWindowGeometry(_aboutDialog);
02393 _preferencesDialog->saveWidgetState(_ui->widget_cloudViewer);
02394 _preferencesDialog->saveWidgetState(_ui->imageView_source);
02395 _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
02396 _preferencesDialog->saveWidgetState(_ui->imageView_odometry);
02397 _preferencesDialog->saveWidgetState(_exportDialog);
02398 _preferencesDialog->saveWidgetState(_postProcessingDialog);
02399 _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
02400 _preferencesDialog->saveSettings();
02401 this->saveFigures();
02402 this->setWindowModified(false);
02403 }
02404
02405 void MainWindow::newDatabase()
02406 {
02407 if(_state != MainWindow::kIdle)
02408 {
02409 UERROR("This method can be called only in IDLE state.");
02410 return;
02411 }
02412 _openedDatabasePath.clear();
02413 _newDatabasePath.clear();
02414 _newDatabasePathOutput.clear();
02415 _databaseUpdated = false;
02416 ULOGGER_DEBUG("");
02417 this->clearTheCache();
02418 std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
02419 if(QFile::exists(databasePath.c_str()))
02420 {
02421 int r = QMessageBox::question(this,
02422 tr("Creating temporary database"),
02423 tr("Cannot create a new database because the temporary database \"%1\" already exists. "
02424 "There may be another instance of RTAB-Map running with the same Working Directory or "
02425 "the last time RTAB-Map was not closed correctly. "
02426 "Do you want to continue (the database will be deleted to create the new one)?").arg(databasePath.c_str()),
02427 QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
02428
02429 if(r == QMessageBox::Yes)
02430 {
02431 if(QFile::remove(databasePath.c_str()))
02432 {
02433 UINFO("Deleted temporary database \"%s\".", databasePath.c_str());
02434 }
02435 else
02436 {
02437 UERROR("Temporary database \"%s\" could not be deleted!", databasePath.c_str());
02438 return;
02439 }
02440 }
02441 else
02442 {
02443 return;
02444 }
02445 }
02446 _newDatabasePath = databasePath.c_str();
02447 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, 0, _preferencesDialog->getAllParameters()));
02448 applyPrefSettings(_preferencesDialog->getAllParameters(), false);
02449 }
02450
02451 void MainWindow::openDatabase()
02452 {
02453 if(_state != MainWindow::kIdle)
02454 {
02455 UERROR("This method can be called only in IDLE state.");
02456 return;
02457 }
02458 _openedDatabasePath.clear();
02459 _newDatabasePath.clear();
02460 _newDatabasePathOutput.clear();
02461 _databaseUpdated = false;
02462 QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
02463 if(!path.isEmpty())
02464 {
02465 this->clearTheCache();
02466 _openedDatabasePath = path;
02467 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, path.toStdString(), 0, _preferencesDialog->getAllParameters()));
02468 }
02469 applyPrefSettings(_preferencesDialog->getAllParameters(), false);
02470 }
02471
02472 bool MainWindow::closeDatabase()
02473 {
02474 if(_state != MainWindow::kInitialized)
02475 {
02476 UERROR("This method can be called only in INITIALIZED state.");
02477 return false;
02478 }
02479
02480 _newDatabasePathOutput.clear();
02481 if(!_newDatabasePath.isEmpty() && _databaseUpdated)
02482 {
02483 QMessageBox::Button b = QMessageBox::question(this,
02484 tr("Save database"),
02485 tr("Save the new database?"),
02486 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard,
02487 QMessageBox::Save);
02488
02489 if(b == QMessageBox::Save)
02490 {
02491
02492 QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
02493
02494 bool ok = true;
02495 newName = QInputDialog::getText(this,
02496 tr("Save database"),
02497 tr("Database name:"),
02498 QLineEdit::Normal,
02499 newName,
02500 &ok);
02501 while(ok)
02502 {
02503 QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
02504 if(QFile::exists(newPath))
02505 {
02506 QMessageBox::StandardButton b = QMessageBox::question(this,
02507 tr("Saving database..."),
02508 tr("Database \"%1\" already exists, do you want to overwrite?").arg(newName+".db"),
02509 QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
02510 if(b == QMessageBox::Yes && QFile::remove(newPath))
02511 {
02512 UINFO("Deleted database \"%s\".", newPath.toStdString().c_str());
02513 _newDatabasePathOutput = newPath;
02514 break;
02515 }
02516 else if(b == QMessageBox::Yes)
02517 {
02518 UERROR("Failed to erase database \"%s\"! Asking for new name...", newPath.toStdString().c_str());
02519 }
02520 }
02521 else
02522 {
02523 _newDatabasePathOutput = newPath;
02524 break;
02525 }
02526
02527 newName = QInputDialog::getText(this,
02528 tr("Save database"),
02529 tr("Database name:"),
02530 QLineEdit::Normal,
02531 newName,
02532 &ok);
02533 }
02534 if(!ok)
02535 {
02536 return false;
02537 }
02538 }
02539 else if(b != QMessageBox::Discard)
02540 {
02541 return false;
02542 }
02543 }
02544
02545 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose));
02546 return true;
02547 }
02548
02549 void MainWindow::editDatabase()
02550 {
02551 if(_state != MainWindow::kIdle)
02552 {
02553 UERROR("This method can be called only in IDLE state.");
02554 return;
02555 }
02556 QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
02557 if(!path.isEmpty())
02558 {
02559 DatabaseViewer * viewer = new DatabaseViewer(this);
02560 viewer->setWindowModality(Qt::WindowModal);
02561 viewer->setAttribute(Qt::WA_DeleteOnClose, true);
02562 viewer->showCloseButton();
02563 if(viewer->openDatabase(path))
02564 {
02565 if(viewer->isSavedMaximized())
02566 {
02567 viewer->showMaximized();
02568 }
02569 else
02570 {
02571 viewer->show();
02572 }
02573 }
02574 else
02575 {
02576 delete viewer;
02577 }
02578 }
02579 }
02580
02581 void MainWindow::startDetection()
02582 {
02583 ParametersMap parameters = _preferencesDialog->getAllParameters();
02584
02585 if((_preferencesDialog->isSourceImageUsed() &&
02586 (_preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages ||
02587 _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo))
02588 ||
02589 _preferencesDialog->isSourceDatabaseUsed())
02590 {
02591 float inputRate = _preferencesDialog->getGeneralInputRate();
02592 float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
02593 int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
02594 if((detectionRate!=0.0f && detectionRate < inputRate) || (detectionRate > 0.0f && inputRate == 0.0f))
02595 {
02596 int button = QMessageBox::question(this,
02597 tr("Incompatible frame rates!"),
02598 tr("\"Source/Input rate\" (%1 Hz) is higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the "
02599 "source input is a directory of images/video/database, some images may be "
02600 "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over "
02601 "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to "
02602 "start the detection anyway?").arg(inputRate).arg(detectionRate),
02603 QMessageBox::Yes | QMessageBox::No);
02604 if(button == QMessageBox::No)
02605 {
02606 return;
02607 }
02608 }
02609 if(bufferingSize != 0)
02610 {
02611 int button = QMessageBox::question(this,
02612 tr("Some images may be skipped!"),
02613 tr("\"RTAB-Map/Images buffer size\" is not infinite (size=%1). As the "
02614 "source input is a directory of images/video/database, some images may be "
02615 "skipped by the detector if the \"Source/Input rate\" (which is %2 Hz) is higher than the "
02616 "rate at which RTAB-Map can process the images. You may want to set the "
02617 "\"RTAB-Map/Images buffer size\" to 0 (infinite) to guaranty that all "
02618 "images are processed. Would you want to start the detection "
02619 "anyway?").arg(bufferingSize).arg(inputRate),
02620 QMessageBox::Yes | QMessageBox::No);
02621 if(button == QMessageBox::No)
02622 {
02623 return;
02624 }
02625 }
02626 }
02627
02628 if(!_preferencesDialog->isCloudsShown(0) || !_preferencesDialog->isScansShown(0))
02629 {
02630 QMessageBox::information(this,
02631 tr("Some data may not be shown!"),
02632 tr("Note that clouds and/or scans visibility settings are set to "
02633 "OFF (see General->\"3D Rendering\" section under Map column)."));
02634 }
02635
02636 UDEBUG("");
02637 emit stateChanged(kStartingDetection);
02638
02639 if(_camera != 0)
02640 {
02641 QMessageBox::warning(this,
02642 tr("RTAB-Map"),
02643 tr("A camera is running, stop it first."));
02644 UWARN("_camera is not null... it must be stopped first");
02645 emit stateChanged(kInitialized);
02646 return;
02647 }
02648 if(_dbReader != 0)
02649 {
02650 QMessageBox::warning(this,
02651 tr("RTAB-Map"),
02652 tr("A database reader is running, stop it first."));
02653 UWARN("_dbReader is not null... it must be stopped first");
02654 emit stateChanged(kInitialized);
02655 return;
02656 }
02657
02658
02659 if( !_preferencesDialog->isSourceImageUsed() &&
02660 !_preferencesDialog->isSourceDatabaseUsed() &&
02661 !_preferencesDialog->isSourceRGBDUsed())
02662 {
02663 QMessageBox::warning(this,
02664 tr("RTAB-Map"),
02665 tr("No sources are selected. See Preferences->Source panel."));
02666 UWARN("No sources are selected. See Preferences->Source panel.");
02667 emit stateChanged(kInitialized);
02668 return;
02669 }
02670
02671 if(_preferencesDialog->isSourceRGBDUsed())
02672 {
02673 CameraRGBD * camera = _preferencesDialog->createCameraRGBD();
02674
02675 if(!camera->init(_preferencesDialog->getCameraInfoDir().toStdString()))
02676 {
02677 ULOGGER_WARN("init camera failed... ");
02678 QMessageBox::warning(this,
02679 tr("RTAB-Map"),
02680 tr("Camera initialization failed..."));
02681 emit stateChanged(kInitialized);
02682 delete camera;
02683 camera = 0;
02684 if(_odomThread)
02685 {
02686 delete _odomThread;
02687 _odomThread = 0;
02688 }
02689 return;
02690 }
02691 else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
02692 {
02693 ((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
02694 ((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
02695 ((CameraOpenNI2*)camera)->setMirroring(_preferencesDialog->getSourceOpenni2Mirroring());
02696 if(CameraOpenNI2::exposureGainAvailable())
02697 {
02698 ((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
02699 ((CameraOpenNI2*)camera)->setGain(_preferencesDialog->getSourceOpenni2Gain());
02700 }
02701 }
02702 camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
02703 camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
02704
02705 _camera = new CameraThread(camera);
02706
02707
02708 if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
02709 {
02710
02711 if(!camera->isCalibrated())
02712 {
02713 UWARN("Camera is not calibrated!");
02714 emit stateChanged(kInitialized);
02715 delete _camera;
02716 _camera = 0;
02717
02718 int button = QMessageBox::question(this,
02719 tr("Camera is not calibrated!"),
02720 tr("RTAB-Map cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
02721 QMessageBox::Yes | QMessageBox::No);
02722 if(button == QMessageBox::Yes)
02723 {
02724 QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
02725 }
02726 return;
02727 }
02728 else
02729 {
02730 if(_odomThread)
02731 {
02732 UERROR("OdomThread must be already deleted here?!");
02733 delete _odomThread;
02734 }
02735 Odometry * odom;
02736 if(_preferencesDialog->getOdomStrategy() == 1)
02737 {
02738 odom = new OdometryOpticalFlow(parameters);
02739 }
02740 else if(_preferencesDialog->getOdomStrategy() == 2)
02741 {
02742 odom = new OdometryMono(parameters);
02743 }
02744 else
02745 {
02746 odom = new OdometryBOW(parameters);
02747 }
02748 _odomThread = new OdometryThread(odom);
02749
02750 UEventsManager::addHandler(_odomThread);
02751 UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
02752 _odomThread->start();
02753 }
02754 }
02755 }
02756 else if(_preferencesDialog->isSourceDatabaseUsed())
02757 {
02758 _dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
02759 _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
02760 _preferencesDialog->getSourceDatabaseOdometryIgnored(),
02761 _preferencesDialog->getSourceDatabaseGoalDelayIgnored());
02762
02763
02764 if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&
02765 _preferencesDialog->getSourceDatabaseOdometryIgnored())
02766 {
02767 if(_odomThread)
02768 {
02769 UERROR("OdomThread must be already deleted here?!");
02770 delete _odomThread;
02771 }
02772 Odometry * odom;
02773 if(_preferencesDialog->getOdomStrategy() == 1)
02774 {
02775 odom = new OdometryOpticalFlow(parameters);
02776 }
02777 else if(_preferencesDialog->getOdomStrategy() == 2)
02778 {
02779 odom = new OdometryMono(parameters);
02780 }
02781 else
02782 {
02783 odom = new OdometryBOW(parameters);
02784 }
02785 _odomThread = new OdometryThread(odom);
02786
02787 UEventsManager::addHandler(_odomThread);
02788 _odomThread->start();
02789 }
02790
02791 if(!_dbReader->init(_preferencesDialog->getSourceDatabaseStartPos()))
02792 {
02793 ULOGGER_WARN("init DBReader failed... ");
02794 QMessageBox::warning(this,
02795 tr("RTAB-Map"),
02796 tr("Database reader initialization failed..."));
02797 emit stateChanged(kInitialized);
02798 delete _dbReader;
02799 _dbReader = 0;
02800 if(_odomThread)
02801 {
02802 delete _odomThread;
02803 _odomThread = 0;
02804 }
02805 return;
02806 }
02807
02808 if(_odomThread)
02809 {
02810 UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
02811 }
02812 }
02813 else
02814 {
02815 if(_preferencesDialog->isSourceImageUsed())
02816 {
02817 Camera * camera = 0;
02818
02819
02820 int sourceType = _preferencesDialog->getSourceImageType();
02821 UASSERT(sourceType >= PreferencesDialog::kSrcUsbDevice && sourceType <= PreferencesDialog::kSrcVideo);
02822 if(sourceType == PreferencesDialog::kSrcImages)
02823 {
02824 camera = new CameraImages(
02825 _preferencesDialog->getSourceImagesPath().append(QDir::separator()).toStdString(),
02826 _preferencesDialog->getSourceImagesStartPos(),
02827 _preferencesDialog->getSourceImagesRefreshDir(),
02828 _preferencesDialog->getGeneralInputRate(),
02829 _preferencesDialog->getSourceWidth(),
02830 _preferencesDialog->getSourceHeight());
02831 }
02832 else if(sourceType == PreferencesDialog::kSrcVideo)
02833 {
02834 camera = new CameraVideo(
02835 _preferencesDialog->getSourceVideoPath().toStdString(),
02836 _preferencesDialog->getGeneralInputRate(),
02837 _preferencesDialog->getSourceWidth(),
02838 _preferencesDialog->getSourceHeight());
02839 }
02840 else
02841 {
02842 camera = new CameraVideo(
02843 _preferencesDialog->getSourceUsbDeviceId(),
02844 _preferencesDialog->getGeneralInputRate(),
02845 _preferencesDialog->getSourceWidth(),
02846 _preferencesDialog->getSourceHeight());
02847 }
02848
02849 if(!camera->init())
02850 {
02851 ULOGGER_WARN("init camera failed... ");
02852 QMessageBox::warning(this,
02853 tr("RTAB-Map"),
02854 tr("Camera initialization failed..."));
02855 emit stateChanged(kInitialized);
02856 delete camera;
02857 camera = 0;
02858 return;
02859 }
02860 camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
02861
02862 _camera = new CameraThread(camera);
02863 }
02864 }
02865
02866 if(_dataRecorder)
02867 {
02868 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
02869 }
02870
02871 _lastOdomPose.setNull();
02872 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer));
02873 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
02874
02875 if(_odomThread)
02876 {
02877 _ui->actionReset_Odometry->setEnabled(true);
02878 }
02879
02880 if(!_preferencesDialog->isStatisticsPublished())
02881 {
02882 QMessageBox::information(this,
02883 tr("Information"),
02884 tr("Note that publishing statistics is disabled, "
02885 "progress will not be shown in the GUI."));
02886 }
02887
02888 emit stateChanged(kDetecting);
02889 }
02890
02891
02892 void MainWindow::pauseDetection()
02893 {
02894 if(_camera || _dbReader)
02895 {
02896 if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
02897 {
02898
02899 emit stateChanged(kPaused);
02900 if(_preferencesDialog->getGeneralInputRate())
02901 {
02902 QTimer::singleShot(1000.0/_preferencesDialog->getGeneralInputRate() + 10, this, SLOT(pauseDetection()));
02903 }
02904 else
02905 {
02906 emit stateChanged(kPaused);
02907 }
02908 }
02909 else
02910 {
02911 emit stateChanged(kPaused);
02912 }
02913 }
02914 else if(_state == kMonitoring)
02915 {
02916 UINFO("Sending pause event!");
02917 emit stateChanged(kMonitoringPaused);
02918 }
02919 else if(_state == kMonitoringPaused)
02920 {
02921 UINFO("Sending unpause event!");
02922 emit stateChanged(kMonitoring);
02923 }
02924 }
02925
02926 void MainWindow::stopDetection()
02927 {
02928 if(!_camera && !_dbReader && !_odomThread)
02929 {
02930 return;
02931 }
02932
02933 if(_state == kDetecting &&
02934 ( (_camera && _camera->isRunning()) ||
02935 (_dbReader && _dbReader->isRunning()) ) )
02936 {
02937 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);
02938
02939 if(button != QMessageBox::Yes)
02940 {
02941 return;
02942 }
02943 }
02944
02945 ULOGGER_DEBUG("");
02946
02947 if(_camera)
02948 {
02949 _camera->join(true);
02950 }
02951
02952 if(_dbReader)
02953 {
02954 _dbReader->join(true);
02955 }
02956
02957 if(_odomThread)
02958 {
02959 _ui->actionReset_Odometry->setEnabled(false);
02960 _odomThread->kill();
02961 }
02962
02963
02964 if(_camera)
02965 {
02966 delete _camera;
02967 _camera = 0;
02968 }
02969 if(_dbReader)
02970 {
02971 delete _dbReader;
02972 _dbReader = 0;
02973 }
02974 if(_odomThread)
02975 {
02976 delete _odomThread;
02977 _odomThread = 0;
02978 }
02979
02980 if(_dataRecorder)
02981 {
02982 delete _dataRecorder;
02983 _dataRecorder = 0;
02984 }
02985
02986 emit stateChanged(kInitialized);
02987 }
02988
02989 void MainWindow::printLoopClosureIds()
02990 {
02991 _ui->dockWidget_console->show();
02992 QString msgRef;
02993 QString msgLoop;
02994 for(int i = 0; i<_refIds.size(); ++i)
02995 {
02996 msgRef.append(QString::number(_refIds[i]));
02997 msgLoop.append(QString::number(_loopClosureIds[i]));
02998 if(i < _refIds.size() - 1)
02999 {
03000 msgRef.append(" ");
03001 msgLoop.append(" ");
03002 }
03003 }
03004 _ui->widget_console->appendMsg(QString("IDs = [%1];").arg(msgRef));
03005 _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
03006 }
03007
03008 void MainWindow::generateMap()
03009 {
03010 if(_graphSavingFileName.isEmpty())
03011 {
03012 _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
03013 }
03014 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
03015 if(!path.isEmpty())
03016 {
03017 _graphSavingFileName = path;
03018 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, path.toStdString()));
03019
03020 _ui->dockWidget_console->show();
03021 _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
03022 }
03023 }
03024
03025 void MainWindow::generateLocalMap()
03026 {
03027 if(_graphSavingFileName.isEmpty())
03028 {
03029 _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
03030 }
03031
03032 bool ok = false;
03033 int loopId = 1;
03034 if(_ui->label_matchId->text().size())
03035 {
03036 std::list<std::string> values = uSplitNumChar(_ui->label_matchId->text().toStdString());
03037 if(values.size() > 1)
03038 {
03039 int val = QString((++values.begin())->c_str()).toInt(&ok);
03040 if(ok)
03041 {
03042 loopId = val;
03043 }
03044 ok = false;
03045 }
03046 }
03047 int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), loopId, 1, 999999, 1, &ok);
03048 if(ok)
03049 {
03050 int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
03051 if(ok)
03052 {
03053 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
03054 if(!path.isEmpty())
03055 {
03056 _graphSavingFileName = path;
03057 QString str = path + QString(";") + QString::number(id) + QString(";") + QString::number(margin);
03058 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTLocalGraph, str.toStdString()));
03059
03060 _ui->dockWidget_console->show();
03061 _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
03062 }
03063 }
03064 }
03065 }
03066
03067 void MainWindow::generateTOROMap()
03068 {
03069 if(_toroSavingFileName.isEmpty())
03070 {
03071 _toroSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "toro.graph";
03072 }
03073
03074 QStringList items;
03075 items.append("Local map optimized");
03076 items.append("Local map not optimized");
03077 items.append("Global map optimized");
03078 items.append("Global map not optimized");
03079 bool ok;
03080 QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
03081 if(ok)
03082 {
03083 bool optimized=false, global=false;
03084 if(item.compare("Local map optimized") == 0)
03085 {
03086 optimized = true;
03087 }
03088 else if(item.compare("Local map not optimized") == 0)
03089 {
03090
03091 }
03092 else if(item.compare("Global map optimized") == 0)
03093 {
03094 global=true;
03095 optimized=true;
03096 }
03097 else if(item.compare("Global map not optimized") == 0)
03098 {
03099 global=true;
03100 }
03101 else
03102 {
03103 UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
03104 }
03105
03106 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _toroSavingFileName, tr("TORO file (*.graph)"));
03107 if(!path.isEmpty())
03108 {
03109 _toroSavingFileName = path;
03110 if(global)
03111 {
03112 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphGlobal, path.toStdString(), optimized?1:0));
03113 }
03114 else
03115 {
03116 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphLocal, path.toStdString(), optimized?1:0));
03117 }
03118
03119 _ui->dockWidget_console->show();
03120 _ui->widget_console->appendMsg(QString("TORO Graph saved (global=%1, optimized=%2)... %3")
03121 .arg(global?"true":"false").arg(optimized?"true":"false").arg(_toroSavingFileName));
03122 }
03123
03124 }
03125 }
03126
03127 void MainWindow::postProcessing()
03128 {
03129 if(_cachedSignatures.size() == 0)
03130 {
03131 UERROR("Signatures must be cached in the GUI to post processing.");
03132 return;
03133 }
03134 if(_postProcessingDialog->exec() != QDialog::Accepted)
03135 {
03136 return;
03137 }
03138
03139 bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
03140 bool reextractFeatures = _postProcessingDialog->isReextractFeatures();
03141 bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
03142 bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
03143 double clusterRadius = _postProcessingDialog->clusterRadius();
03144 double clusterAngle = _postProcessingDialog->clusterAngle();
03145 int detectLoopClosureIterations = _postProcessingDialog->iterations();
03146
03147 if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks)
03148 {
03149 UWARN("No post-processing selection...");
03150 return;
03151 }
03152
03153
03154 bool allDataAvailable = true;
03155 std::map<int, Transform> odomPoses;
03156 for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
03157 iter!=_currentPosesMap.end() && allDataAvailable;
03158 ++iter)
03159 {
03160 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
03161 if(jter != _cachedSignatures.end())
03162 {
03163 if(jter->getPose().isNull())
03164 {
03165 UWARN("Odometry pose of %d is null.", iter->first);
03166 allDataAvailable = false;
03167 }
03168 else
03169 {
03170 odomPoses.insert(*iter);
03171 }
03172 if(jter->getLocalTransform().isNull())
03173 {
03174 UWARN("Local transform of %d is null.", iter->first);
03175 allDataAvailable = false;
03176 }
03177 if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
03178 {
03179
03180 if(jter->getDepthCompressed().empty() || jter->getFx() <= 0.0f || jter->getFy() <= 0.0f)
03181 {
03182 UWARN("Depth data of %d missing.", iter->first);
03183 allDataAvailable = false;
03184 }
03185
03186 if(reextractFeatures)
03187 {
03188
03189 if(jter->getImageCompressed().empty())
03190 {
03191 UWARN("Rgb of %d missing.", iter->first);
03192 allDataAvailable = false;
03193 }
03194 }
03195 }
03196 }
03197 else
03198 {
03199 UWARN("Node %d missing.", iter->first);
03200 allDataAvailable = false;
03201 }
03202 }
03203
03204 if(!allDataAvailable)
03205 {
03206 QMessageBox::warning(this, tr("Not all data available in the GUI..."),
03207 tr("Some data missing in the cache to respect the constraints chosen. "
03208 "Try \"Edit->Download all clouds\" to update the cache and try again."));
03209 return;
03210 }
03211
03212 _initProgressDialog->setAutoClose(false, 1);
03213 _initProgressDialog->resetProgress();
03214 _initProgressDialog->clear();
03215 _initProgressDialog->show();
03216 _initProgressDialog->appendText("Post-processing beginning!");
03217
03218 int totalSteps = 0;
03219 if(refineNeighborLinks)
03220 {
03221 totalSteps+=(int)odomPoses.size();
03222 }
03223 if(refineLoopClosureLinks)
03224 {
03225 totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
03226 }
03227 _initProgressDialog->setMaximumSteps(totalSteps);
03228 _initProgressDialog->show();
03229
03230 ParametersMap parameters = _preferencesDialog->getAllParameters();
03231 graph::Optimizer * optimizer = graph::Optimizer::create(parameters);
03232 bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
03233 Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
03234
03235 int loopClosuresAdded = 0;
03236 if(detectMoreLoopClosures)
03237 {
03238 Memory memory(parameters);
03239 if(reextractFeatures)
03240 {
03241 ParametersMap customParameters;
03242
03243 customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
03244 customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
03245 customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
03246 customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
03247 customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
03248 customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kLccReextractNNDR())));
03249 customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kLccReextractFeatureType())));
03250 customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kLccReextractMaxWords())));
03251 customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
03252 memory.parseParameters(customParameters);
03253 }
03254
03255 UASSERT(detectLoopClosureIterations>0);
03256 for(int n=0; n<detectLoopClosureIterations; ++n)
03257 {
03258 _initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
03259 .arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
03260
03261 std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
03262 _currentPosesMap,
03263 clusterRadius,
03264 clusterAngle*CV_PI/180.0);
03265
03266 _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+(int)clusters.size());
03267 _initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
03268
03269 int i=0;
03270 std::set<int> addedLinks;
03271 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
03272 {
03273 int from = iter->first;
03274 int to = iter->second;
03275 if(iter->first < iter->second)
03276 {
03277 from = iter->second;
03278 to = iter->first;
03279 }
03280
03281
03282 if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
03283 rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
03284 {
03285 if(!_cachedSignatures.contains(from))
03286 {
03287 UERROR("Didn't find signature %d", from);
03288 }
03289 else if(!_cachedSignatures.contains(to))
03290 {
03291 UERROR("Didn't find signature %d", to);
03292 }
03293 else
03294 {
03295 _initProgressDialog->incrementStep();
03296 QApplication::processEvents();
03297
03298 Signature & signatureFrom = _cachedSignatures[from];
03299 Signature & signatureTo = _cachedSignatures[to];
03300
03301 Transform transform;
03302 std::string rejectedMsg;
03303 int inliers = -1;
03304 double variance = -1.0;
03305 if(reextractFeatures)
03306 {
03307 memory.init("", true);
03308
03309
03310 SensorData dataFrom = signatureFrom.toSensorData();
03311 SensorData dataTo = signatureTo.toSensorData();
03312
03313 if(dataFrom.isValid() &&
03314 dataFrom.isMetric() &&
03315 dataTo.isValid() &&
03316 dataTo.isMetric() &&
03317 dataFrom.id() != Memory::kIdInvalid &&
03318 signatureFrom.id() != Memory::kIdInvalid)
03319 {
03320 if(from > to)
03321 {
03322 memory.update(dataTo);
03323 memory.update(dataFrom);
03324 }
03325 else
03326 {
03327 memory.update(dataFrom);
03328 memory.update(dataTo);
03329 }
03330
03331 transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance);
03332 }
03333 else
03334 {
03335 UERROR("not supposed to be here!");
03336 }
03337 }
03338 else
03339 {
03340 transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance);
03341 }
03342 if(!transform.isNull())
03343 {
03344 UINFO("Added new loop closure between %d and %d.", from, to);
03345 addedLinks.insert(from);
03346 addedLinks.insert(to);
03347 _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance, variance)));
03348 ++loopClosuresAdded;
03349 _initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
03350 }
03351 }
03352 }
03353 }
03354 _initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
03355 .arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
03356 if(addedLinks.size() == 0)
03357 {
03358 break;
03359 }
03360
03361 if(n+1 < detectLoopClosureIterations)
03362 {
03363 _initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
03364 .arg(odomPoses.size()).arg(_currentLinksMap.size()));
03365 int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
03366 std::map<int, rtabmap::Transform> posesOut;
03367 std::multimap<int, rtabmap::Link> linksOut;
03368 std::map<int, rtabmap::Transform> optimizedPoses;
03369 optimizer->getConnectedGraph(
03370 fromId,
03371 odomPoses,
03372 _currentLinksMap,
03373 posesOut,
03374 linksOut);
03375 optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
03376 _currentPosesMap = optimizedPoses;
03377 _initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
03378 }
03379 }
03380 UINFO("Added %d loop closures.", loopClosuresAdded);
03381 _initProgressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
03382 }
03383
03384 if(refineNeighborLinks || refineLoopClosureLinks)
03385 {
03386 if(refineLoopClosureLinks)
03387 {
03388 _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
03389 }
03390 _initProgressDialog->appendText(tr("Refining links..."));
03391
03392 int decimation=8;
03393 float maxDepth=2.0f;
03394 float voxelSize=0.01f;
03395 int samples = 0;
03396 float maxCorrespondences = 0.05f;
03397 float correspondenceRatio = 0.7f;
03398 float icpIterations = 30;
03399 Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
03400 Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
03401 Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
03402 Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
03403 Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
03404 Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
03405 Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
03406 bool pointToPlane = false;
03407 int pointToPlaneNormalNeighbors = 20;
03408 Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
03409 Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
03410
03411 int i=0;
03412 for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
03413 {
03414 int type = iter->second.type();
03415
03416 if((refineNeighborLinks && type==Link::kNeighbor) ||
03417 (refineLoopClosureLinks && type!=Link::kNeighbor))
03418 {
03419 int from = iter->second.from();
03420 int to = iter->second.to();
03421
03422 _initProgressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
03423 _initProgressDialog->incrementStep();
03424 QApplication::processEvents();
03425
03426 if(!_cachedSignatures.contains(from))
03427 {
03428 UERROR("Didn't find signature %d",from);
03429 }
03430 else if(!_cachedSignatures.contains(to))
03431 {
03432 UERROR("Didn't find signature %d", to);
03433 }
03434 else
03435 {
03436 Signature & signatureFrom = _cachedSignatures[from];
03437 Signature & signatureTo = _cachedSignatures[to];
03438
03439
03440 cv::Mat depthA, depthB;
03441 signatureFrom.uncompressData(0, &depthA, 0);
03442 signatureTo.uncompressData(0, &depthB, 0);
03443
03444 if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
03445 {
03446 QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
03447 UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP...");
03448 break;
03449 }
03450
03451 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
03452 signatureFrom.getFx(), signatureFrom.getFy(), signatureFrom.getCx(), signatureFrom.getCy(),
03453 decimation,
03454 maxDepth,
03455 voxelSize,
03456 samples,
03457 signatureFrom.getLocalTransform());
03458 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
03459 signatureTo.getFx(), signatureTo.getFy(), signatureTo.getCx(), signatureTo.getCy(),
03460 decimation,
03461 maxDepth,
03462 voxelSize,
03463 samples,
03464 iter->second.transform() * signatureTo.getLocalTransform());
03465
03466 bool hasConverged = false;
03467 double variance = -1;
03468 int correspondences = 0;
03469 Transform transform;
03470 if(pointToPlane)
03471 {
03472 pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
03473 pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
03474
03475 cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
03476 if(cloudA->size() != cloudANormals->size())
03477 {
03478 UWARN("removed nan normals...");
03479 }
03480
03481 cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
03482 if(cloudB->size() != cloudBNormals->size())
03483 {
03484 UWARN("removed nan normals...");
03485 }
03486
03487 transform = util3d::icpPointToPlane(cloudBNormals,
03488 cloudANormals,
03489 maxCorrespondences,
03490 icpIterations,
03491 &hasConverged,
03492 &variance,
03493 &correspondences);
03494 }
03495 else
03496 {
03497 transform = util3d::icp(cloudB,
03498 cloudA,
03499 maxCorrespondences,
03500 icpIterations,
03501 &hasConverged,
03502 &variance,
03503 &correspondences);
03504 }
03505
03506 float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
03507
03508 if(!transform.isNull() && hasConverged &&
03509 correspondencesRatio >= correspondenceRatio)
03510 {
03511 Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
03512 iter->second = newLink;
03513 }
03514 else
03515 {
03516 QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
03517 _initProgressDialog->appendText(str, Qt::darkYellow);
03518 UWARN("%s", str.toStdString().c_str());
03519 }
03520 }
03521 }
03522 }
03523 _initProgressDialog->appendText(tr("Refining links...done!"));
03524 }
03525
03526 _initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
03527 .arg(odomPoses.size()).arg(_currentLinksMap.size()));
03528
03529 int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
03530 std::map<int, rtabmap::Transform> posesOut;
03531 std::multimap<int, rtabmap::Link> linksOut;
03532 std::map<int, rtabmap::Transform> optimizedPoses;
03533 optimizer->getConnectedGraph(
03534 fromId,
03535 odomPoses,
03536 _currentLinksMap,
03537 posesOut,
03538 linksOut);
03539 optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
03540 _initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
03541 _initProgressDialog->incrementStep();
03542
03543 _initProgressDialog->appendText(tr("Updating map..."));
03544 this->updateMapCloud(optimizedPoses, Transform(), std::multimap<int, Link>(_currentLinksMap), std::map<int, int>(_currentMapIds), false);
03545 _initProgressDialog->appendText(tr("Updating map... done!"));
03546
03547 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
03548 _initProgressDialog->appendText("Post-processing finished!");
03549
03550 delete optimizer;
03551 }
03552
03553 void MainWindow::deleteMemory()
03554 {
03555 QMessageBox::StandardButton button;
03556 if(_state == kMonitoring || _state == kMonitoringPaused)
03557 {
03558 button = QMessageBox::question(this,
03559 tr("Deleting memory..."),
03560 tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
03561 QMessageBox::Yes|QMessageBox::No,
03562 QMessageBox::No);
03563 }
03564 else
03565 {
03566 button = QMessageBox::question(this,
03567 tr("Deleting memory..."),
03568 tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
03569 QMessageBox::Yes|QMessageBox::No,
03570 QMessageBox::No);
03571 }
03572
03573 if(button != QMessageBox::Yes)
03574 {
03575 return;
03576 }
03577
03578 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
03579 if(_state!=kDetecting)
03580 {
03581 _databaseUpdated = false;
03582 }
03583 this->clearTheCache();
03584 }
03585
03586 QString MainWindow::getWorkingDirectory() const
03587 {
03588 return _preferencesDialog->getWorkingDirectory();
03589 }
03590
03591 void MainWindow::openWorkingDirectory()
03592 {
03593 QString filePath = _preferencesDialog->getWorkingDirectory();
03594 #if defined(Q_WS_MAC)
03595 QStringList args;
03596 args << "-e";
03597 args << "tell application \"Finder\"";
03598 args << "-e";
03599 args << "activate";
03600 args << "-e";
03601 args << "select POSIX file \""+filePath+"\"";
03602 args << "-e";
03603 args << "end tell";
03604 QProcess::startDetached("osascript", args);
03605 #elif defined(Q_WS_WIN)
03606 QStringList args;
03607 args << "/select," << QDir::toNativeSeparators(filePath);
03608 QProcess::startDetached("explorer", args);
03609 #else
03610 UERROR("Only works on Mac and Windows");
03611 #endif
03612 }
03613
03614 void MainWindow::updateEditMenu()
03615 {
03616
03617 if(_state != kMonitoring && _state != kMonitoringPaused && (!_openedDatabasePath.isEmpty() || !_newDatabasePath.isEmpty()))
03618 {
03619 if(!_openedDatabasePath.isEmpty())
03620 {
03621 _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_openedDatabasePath.toStdString())/1000000));
03622 }
03623 else
03624 {
03625 _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_newDatabasePath.toStdString())/1000000));
03626 }
03627 }
03628 }
03629
03630 void MainWindow::selectImages()
03631 {
03632 _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages);
03633 }
03634
03635 void MainWindow::selectVideo()
03636 {
03637 _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo);
03638 }
03639
03640 void MainWindow::selectStream()
03641 {
03642 _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice);
03643 }
03644
03645 void MainWindow::selectDatabase()
03646 {
03647 _preferencesDialog->selectSourceDatabase(true);
03648 }
03649
03650 void MainWindow::selectOpenni()
03651 {
03652 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
03653 }
03654
03655 void MainWindow::selectFreenect()
03656 {
03657 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
03658 }
03659
03660 void MainWindow::selectOpenniCv()
03661 {
03662 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
03663 }
03664
03665 void MainWindow::selectOpenniCvAsus()
03666 {
03667 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
03668 }
03669
03670 void MainWindow::selectOpenni2()
03671 {
03672 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
03673 }
03674
03675 void MainWindow::selectFreenect2()
03676 {
03677 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect2);
03678 }
03679
03680 void MainWindow::selectStereoDC1394()
03681 {
03682 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394);
03683 }
03684
03685 void MainWindow::selectStereoFlyCapture2()
03686 {
03687 _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoFlyCapture2);
03688 }
03689
03690
03691 void MainWindow::dumpTheMemory()
03692 {
03693 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
03694 }
03695
03696 void MainWindow::dumpThePrediction()
03697 {
03698 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
03699 }
03700
03701 void MainWindow::sendGoal()
03702 {
03703 UINFO("Sending a goal...");
03704 bool ok = false;
03705 int id = QInputDialog::getInt(this, tr("Send a goal"), tr("Goal location ID: "), 1, 1, 99999, 1, &ok);
03706 if(ok)
03707 {
03708 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
03709 UINFO("Posting event with goal %d", id);
03710 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", id));
03711 }
03712 }
03713
03714 void MainWindow::downloadAllClouds()
03715 {
03716 QStringList items;
03717 items.append("Local map optimized");
03718 items.append("Local map not optimized");
03719 items.append("Global map optimized");
03720 items.append("Global map not optimized");
03721
03722 bool ok;
03723 QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
03724 if(ok)
03725 {
03726 bool optimized=false, global=false;
03727 if(item.compare("Local map optimized") == 0)
03728 {
03729 optimized = true;
03730 }
03731 else if(item.compare("Local map not optimized") == 0)
03732 {
03733
03734 }
03735 else if(item.compare("Global map optimized") == 0)
03736 {
03737 global=true;
03738 optimized=true;
03739 }
03740 else if(item.compare("Global map not optimized") == 0)
03741 {
03742 global=true;
03743 }
03744 else
03745 {
03746 UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
03747 }
03748
03749 UINFO("Download clouds...");
03750 _initProgressDialog->setAutoClose(true, 1);
03751 _initProgressDialog->resetProgress();
03752 _initProgressDialog->show();
03753 _initProgressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
03754 .arg(global?"true":"false").arg(optimized?"true":"false"));
03755 if(global)
03756 {
03757 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapGlobal, "", optimized?1:0));
03758 }
03759 else
03760 {
03761 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapLocal, "", optimized?1:0));
03762 }
03763 }
03764 }
03765
03766 void MainWindow::downloadPoseGraph()
03767 {
03768 QStringList items;
03769 items.append("Local map optimized");
03770 items.append("Local map not optimized");
03771 items.append("Global map optimized");
03772 items.append("Global map not optimized");
03773
03774 bool ok;
03775 QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
03776 if(ok)
03777 {
03778 bool optimized=false, global=false;
03779 if(item.compare("Local map optimized") == 0)
03780 {
03781 optimized = true;
03782 }
03783 else if(item.compare("Local map not optimized") == 0)
03784 {
03785
03786 }
03787 else if(item.compare("Global map optimized") == 0)
03788 {
03789 global=true;
03790 optimized=true;
03791 }
03792 else if(item.compare("Global map not optimized") == 0)
03793 {
03794 global=true;
03795 }
03796 else
03797 {
03798 UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
03799 }
03800
03801 UINFO("Download the graph...");
03802 _initProgressDialog->setAutoClose(true, 1);
03803 _initProgressDialog->resetProgress();
03804 _initProgressDialog->show();
03805 _initProgressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
03806 .arg(global?"true":"false").arg(optimized?"true":"false"));
03807 if(global)
03808 {
03809 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphGlobal, "", optimized?1:0));
03810 }
03811 else
03812 {
03813 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphLocal, "", optimized?1:0));
03814 }
03815 }
03816 }
03817
03818 void MainWindow::clearTheCache()
03819 {
03820 _cachedSignatures.clear();
03821 _createdClouds.clear();
03822 _createdScans.clear();
03823 _gridLocalMaps.clear();
03824 _projectionLocalMaps.clear();
03825 _ui->widget_cloudViewer->removeAllClouds();
03826 _ui->widget_cloudViewer->removeAllGraphs();
03827 _ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor());
03828 _ui->widget_cloudViewer->clearTrajectory();
03829 _ui->widget_mapVisibility->clear();
03830 _currentPosesMap.clear();
03831 _currentLinksMap.clear();
03832 _currentMapIds.clear();
03833 _odometryCorrection = Transform::getIdentity();
03834 _lastOdomPose.setNull();
03835
03836 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
03837 _ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
03838 _ui->actionPost_processing->setEnabled(false);
03839 _ui->actionSave_point_cloud->setEnabled(false);
03840 _ui->actionView_scans->setEnabled(false);
03841 _ui->actionView_high_res_point_cloud->setEnabled(false);
03842 _likelihoodCurve->clear();
03843 _rawLikelihoodCurve->clear();
03844 _posteriorCurve->clear();
03845 _lastId = 0;
03846 _lastIds.clear();
03847 _ui->label_stats_loopClosuresDetected->setText("0");
03848 _ui->label_stats_loopClosuresReactivatedDetected->setText("0");
03849 _ui->label_stats_loopClosuresRejected->setText("0");
03850 _refIds.clear();
03851 _loopClosureIds.clear();
03852 _ui->label_refId->clear();
03853 _ui->label_matchId->clear();
03854 _ui->graphicsView_graphView->clearAll();
03855 _ui->imageView_source->clear();
03856 _ui->imageView_loopClosure->clear();
03857 _ui->imageView_odometry->clear();
03858 _ui->imageView_source->setBackgroundColor(Qt::black);
03859 _ui->imageView_loopClosure->setBackgroundColor(Qt::black);
03860 _ui->imageView_odometry->setBackgroundColor(Qt::black);
03861 }
03862
03863 void MainWindow::updateElapsedTime()
03864 {
03865 if(_state == kDetecting || _state == kMonitoring)
03866 {
03867 QString format = "hh:mm:ss";
03868 _ui->label_elapsedTime->setText((QTime().fromString(_ui->label_elapsedTime->text(), format).addMSecs(_elapsedTime->restart())).toString(format));
03869 }
03870 }
03871
03872 void MainWindow::saveFigures()
03873 {
03874 QList<int> curvesPerFigure;
03875 QStringList curveNames;
03876 _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
03877
03878 QStringList curvesPerFigureStr;
03879 for(int i=0; i<curvesPerFigure.size(); ++i)
03880 {
03881 curvesPerFigureStr.append(QString::number(curvesPerFigure[i]));
03882 }
03883 for(int i=0; i<curveNames.size(); ++i)
03884 {
03885 curveNames[i].replace(' ', '_');
03886 }
03887 _preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
03888 _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
03889 }
03890
03891 void MainWindow::loadFigures()
03892 {
03893 QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
03894 QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
03895
03896 if(!curvesPerFigure.isEmpty())
03897 {
03898 QStringList curvesPerFigureList = curvesPerFigure.split(" ");
03899 QStringList curvesNamesList = curveNames.split(" ");
03900
03901 int j=0;
03902 for(int i=0; i<curvesPerFigureList.size(); ++i)
03903 {
03904 bool ok = false;
03905 int count = curvesPerFigureList[i].toInt(&ok);
03906 if(!ok)
03907 {
03908 QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
03909 break;
03910 }
03911 else
03912 {
03913 _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
03914 for(int k=1; k<count && j<curveNames.size(); ++k)
03915 {
03916 _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
03917 }
03918 }
03919 }
03920 }
03921
03922 }
03923
03924 void MainWindow::openPreferences()
03925 {
03926 _preferencesDialog->setMonitoringState(_state == kMonitoring || _state == kMonitoringPaused);
03927 _preferencesDialog->exec();
03928 }
03929
03930 void MainWindow::selectScreenCaptureFormat(bool checked)
03931 {
03932 if(checked)
03933 {
03934 QStringList items;
03935 items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
03936 bool ok;
03937 QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
03938 if(ok && !item.isEmpty())
03939 {
03940 if(item.compare("Synchronize with map update") == 0)
03941 {
03942 _autoScreenCaptureOdomSync = false;
03943 }
03944 else
03945 {
03946 _autoScreenCaptureOdomSync = true;
03947 }
03948 }
03949 else
03950 {
03951 _ui->actionAuto_screen_capture->setChecked(false);
03952 }
03953 }
03954 }
03955
03956 void MainWindow::takeScreenshot()
03957 {
03958 this->captureScreen();
03959 }
03960
03961 void MainWindow::setAspectRatio(int w, int h)
03962 {
03963 QRect rect = this->geometry();
03964 if(h<100 && w<100)
03965 {
03966
03967 if(float(rect.width())/float(rect.height()) > float(w)/float(h))
03968 {
03969 rect.setWidth(w*(rect.height()/h));
03970 rect.setHeight((rect.height()/h)*h);
03971 }
03972 else
03973 {
03974 rect.setHeight(h*(rect.width()/w));
03975 rect.setWidth((rect.width()/w)*w);
03976 }
03977 }
03978 else
03979 {
03980
03981 rect.setWidth(w);
03982 rect.setHeight(h);
03983 }
03984 this->setGeometry(rect);
03985 }
03986
03987 void MainWindow::setAspectRatio16_9()
03988 {
03989 this->setAspectRatio(16, 9);
03990 }
03991
03992 void MainWindow::setAspectRatio16_10()
03993 {
03994 this->setAspectRatio(16, 10);
03995 }
03996
03997 void MainWindow::setAspectRatio4_3()
03998 {
03999 this->setAspectRatio(4, 3);
04000 }
04001
04002 void MainWindow::setAspectRatio240p()
04003 {
04004 this->setAspectRatio((240*16)/9, 240);
04005 }
04006
04007 void MainWindow::setAspectRatio360p()
04008 {
04009 this->setAspectRatio((360*16)/9, 360);
04010 }
04011
04012 void MainWindow::setAspectRatio480p()
04013 {
04014 this->setAspectRatio((480*16)/9, 480);
04015 }
04016
04017 void MainWindow::setAspectRatio720p()
04018 {
04019 this->setAspectRatio((720*16)/9, 720);
04020 }
04021
04022 void MainWindow::setAspectRatio1080p()
04023 {
04024 this->setAspectRatio((1080*16)/9, 1080);
04025 }
04026
04027 void MainWindow::exportGridMap()
04028 {
04029 double gridCellSize = 0.05;
04030 bool ok;
04031 gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
04032 if(!ok)
04033 {
04034 return;
04035 }
04036
04037 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
04038
04039
04040 float xMin=0.0f, yMin=0.0f;
04041 cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
04042 poses,
04043 _preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
04044 gridCellSize,
04045 xMin, yMin,
04046 0,
04047 _preferencesDialog->isGridMapEroded());
04048
04049 if(!pixels.empty())
04050 {
04051 cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
04052
04053 for (int i = 0; i < pixels.rows; ++i)
04054 {
04055 for (int j = 0; j < pixels.cols; ++j)
04056 {
04057 char v = pixels.at<char>(i, j);
04058 unsigned char gray;
04059 if(v == 0)
04060 {
04061 gray = 178;
04062 }
04063 else if(v == 100)
04064 {
04065 gray = 0;
04066 }
04067 else
04068 {
04069 gray = 89;
04070 }
04071 map8U.at<unsigned char>(i, j) = gray;
04072 }
04073 }
04074
04075 QImage image = uCvMat2QImage(map8U, false);
04076
04077 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
04078 if(!path.isEmpty())
04079 {
04080 if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
04081 {
04082
04083 path += ".png";
04084 }
04085
04086 QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
04087 QPixmap::fromImage(img).save(path);
04088
04089 QDesktopServices::openUrl(QUrl::fromLocalFile(path));
04090 }
04091 }
04092 }
04093
04094 void MainWindow::exportScans()
04095 {
04096 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
04097 if(getExportedScans(scans))
04098 {
04099 if(scans.size())
04100 {
04101 QMessageBox::StandardButton b = QMessageBox::question(this,
04102 tr("Binary file?"),
04103 tr("Do you want to save in binary mode?"),
04104 QMessageBox::No | QMessageBox::Yes,
04105 QMessageBox::Yes);
04106
04107 if(b == QMessageBox::No || b == QMessageBox::Yes)
04108 {
04109 this->saveScans(scans, b == QMessageBox::Yes);
04110 }
04111 }
04112 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
04113 }
04114 }
04115
04116 void MainWindow::viewScans()
04117 {
04118 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
04119 if(getExportedScans(scans))
04120 {
04121 QDialog * window = new QDialog(this, Qt::Window);
04122 window->setWindowFlags(Qt::Dialog);
04123 window->setWindowTitle(tr("Scans (%1 nodes)").arg(scans.size()));
04124 window->setMinimumWidth(800);
04125 window->setMinimumHeight(600);
04126
04127 CloudViewer * viewer = new CloudViewer(window);
04128 viewer->setCameraLockZ(false);
04129
04130 QVBoxLayout *layout = new QVBoxLayout();
04131 layout->addWidget(viewer);
04132 window->setLayout(layout);
04133 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
04134
04135 window->show();
04136
04137 uSleep(500);
04138
04139 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr>::iterator iter = scans.begin(); iter!=scans.end(); ++iter)
04140 {
04141 _initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
04142 _initProgressDialog->incrementStep();
04143
04144 QColor color = Qt::red;
04145 int mapId = uValue(_currentMapIds, iter->first, -1);
04146 if(mapId >= 0)
04147 {
04148 color = (Qt::GlobalColor)(mapId % 12 + 7 );
04149 }
04150 viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
04151 _initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
04152 }
04153
04154 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
04155 }
04156 }
04157
04158 bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans)
04159 {
04160 QMessageBox::StandardButton b = QMessageBox::question(this,
04161 tr("Assemble scans?"),
04162 tr("Do you want to assemble the scans in only one cloud?"),
04163 QMessageBox::No | QMessageBox::Yes,
04164 QMessageBox::Yes);
04165
04166 if(b != QMessageBox::No && b != QMessageBox::Yes)
04167 {
04168 return false;
04169 }
04170
04171 double voxel = 0.01;
04172 bool assemble = b == QMessageBox::Yes;
04173
04174 if(assemble)
04175 {
04176 bool ok;
04177 voxel = QInputDialog::getDouble(this, tr("Voxel size"), tr("Voxel size (m):"), voxel, 0.00, 0.1, 2, &ok);
04178 if(!ok)
04179 {
04180 return false;
04181 }
04182 }
04183
04184 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>());
04185 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
04186
04187 _initProgressDialog->setAutoClose(true, 1);
04188 _initProgressDialog->resetProgress();
04189 _initProgressDialog->show();
04190 _initProgressDialog->setMaximumSteps(int(poses.size())*(assemble?1:2)+1);
04191
04192 int count = 1;
04193 int i = 0;
04194 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
04195 {
04196 bool inserted = false;
04197 if(_createdScans.find(iter->first) != _createdScans.end())
04198 {
04199 pcl::PointCloud<pcl::PointXYZ>::Ptr scan = _createdScans.at(iter->first);
04200 if(scan->size())
04201 {
04202 if(assemble)
04203 {
04204 *assembledScans += *util3d::transformPointCloud<pcl::PointXYZ>(scan, iter->second);;
04205
04206 if(count++ % 100 == 0)
04207 {
04208 if(assembledScans->size() && voxel)
04209 {
04210 assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
04211 }
04212 }
04213 }
04214 else
04215 {
04216 scans.insert(std::make_pair(iter->first, scan));
04217 }
04218 inserted = true;
04219 }
04220 }
04221 if(inserted)
04222 {
04223 _initProgressDialog->appendText(tr("Generated scan %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
04224 }
04225 else
04226 {
04227 _initProgressDialog->appendText(tr("Ignored scan %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
04228 }
04229 _initProgressDialog->incrementStep();
04230 QApplication::processEvents();
04231 }
04232
04233 if(assemble)
04234 {
04235 if(voxel && assembledScans->size())
04236 {
04237 assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
04238 }
04239 if(assembledScans->size())
04240 {
04241 scans.insert(std::make_pair(0, assembledScans));
04242 }
04243 }
04244 return true;
04245 }
04246
04247 void MainWindow::exportClouds()
04248 {
04249 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
04250 std::map<int, pcl::PolygonMesh::Ptr> meshes;
04251
04252 if(getExportedClouds(clouds, meshes, true))
04253 {
04254 if(meshes.size())
04255 {
04256 saveMeshes(meshes, _exportDialog->getBinaryFile());
04257 }
04258 else
04259 {
04260 saveClouds(clouds, _exportDialog->getBinaryFile());
04261 }
04262 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
04263 }
04264 }
04265
04266 void MainWindow::viewClouds()
04267 {
04268 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
04269 std::map<int, pcl::PolygonMesh::Ptr> meshes;
04270
04271 if(getExportedClouds(clouds, meshes, false))
04272 {
04273 QDialog * window = new QDialog(this, Qt::Window);
04274 if(meshes.size())
04275 {
04276 window->setWindowTitle(tr("Meshes (%1 nodes)").arg(meshes.size()));
04277 }
04278 else
04279 {
04280 window->setWindowTitle(tr("Clouds (%1 nodes)").arg(clouds.size()));
04281 }
04282 window->setMinimumWidth(800);
04283 window->setMinimumHeight(600);
04284
04285 CloudViewer * viewer = new CloudViewer(window);
04286 viewer->setCameraLockZ(false);
04287
04288 QVBoxLayout *layout = new QVBoxLayout();
04289 layout->addWidget(viewer);
04290 window->setLayout(layout);
04291 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
04292
04293 window->show();
04294
04295 uSleep(500);
04296
04297 if(meshes.size())
04298 {
04299 for(std::map<int, pcl::PolygonMesh::Ptr>::iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
04300 {
04301 _initProgressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->polygons.size()));
04302 _initProgressDialog->incrementStep();
04303 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
04304 pcl::fromPCLPointCloud2(iter->second->cloud, *cloud);
04305 viewer->addCloudMesh(uFormat("mesh%d",iter->first), cloud, iter->second->polygons, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
04306 _initProgressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)... done.").arg(iter->first).arg(iter->second->polygons.size()));
04307 QApplication::processEvents();
04308 }
04309 }
04310 else if(clouds.size())
04311 {
04312 for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
04313 {
04314 _initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
04315 _initProgressDialog->incrementStep();
04316
04317 QColor color = Qt::gray;
04318 int mapId = uValue(_currentMapIds, iter->first, -1);
04319 if(mapId >= 0)
04320 {
04321 color = (Qt::GlobalColor)(mapId % 12 + 7 );
04322 }
04323 viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
04324 _initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
04325 }
04326 }
04327
04328 _initProgressDialog->setValue(_initProgressDialog->maximumSteps());
04329 }
04330 }
04331
04332 bool MainWindow::getExportedClouds(
04333 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
04334 std::map<int, pcl::PolygonMesh::Ptr> & meshes,
04335 bool toSave)
04336 {
04337 if(_exportDialog->isVisible())
04338 {
04339 return false;
04340 }
04341 if(toSave)
04342 {
04343 _exportDialog->setSaveButton();
04344 }
04345 else
04346 {
04347 _exportDialog->setOkButton();
04348 }
04349 _exportDialog->enableRegeneration(_preferencesDialog->isImagesKept());
04350 if(_exportDialog->exec() == QDialog::Accepted)
04351 {
04352 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
04353
04354 _initProgressDialog->setAutoClose(true, 1);
04355 _initProgressDialog->resetProgress();
04356 _initProgressDialog->show();
04357 int mul = _exportDialog->getMesh()&&!_exportDialog->getGenerate()?3:_exportDialog->getMLS()&&!_exportDialog->getGenerate()?2:1;
04358 _initProgressDialog->setMaximumSteps(int(poses.size())*mul+1);
04359
04360 if(_exportDialog->getAssemble())
04361 {
04362 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->getAssembledCloud(
04363 poses,
04364 _exportDialog->getAssembleVoxel(),
04365 _exportDialog->getGenerate(),
04366 _exportDialog->getGenerateDecimation(),
04367 _exportDialog->getGenerateVoxel(),
04368 _exportDialog->getGenerateMaxDepth());
04369
04370 clouds.insert(std::make_pair(0, cloud));
04371 }
04372 else
04373 {
04374 clouds = this->getClouds(
04375 poses,
04376 _exportDialog->getGenerate(),
04377 _exportDialog->getGenerateDecimation(),
04378 _exportDialog->getGenerateVoxel(),
04379 _exportDialog->getGenerateMaxDepth());
04380 }
04381
04382 if(_exportDialog->getMLS() || _exportDialog->getMesh())
04383 {
04384 for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter=clouds.begin();
04385 iter!= clouds.end();
04386 ++iter)
04387 {
04388 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
04389 if(_exportDialog->getMLS())
04390 {
04391 _initProgressDialog->appendText(tr("Smoothing the surface of cloud %1 using Moving Least Squares (MLS) algorithm... "
04392 "[search radius=%2m]").arg(iter->first).arg(_exportDialog->getMLSRadius()));
04393 _initProgressDialog->incrementStep();
04394 QApplication::processEvents();
04395
04396 cloudWithNormals = util3d::computeNormalsSmoothed(iter->second, (float)_exportDialog->getMLSRadius());
04397
04398 iter->second->clear();
04399 pcl::copyPointCloud(*cloudWithNormals, *iter->second);
04400 }
04401 else if(_exportDialog->getMesh())
04402 {
04403 _initProgressDialog->appendText(tr("Computing surface normals of cloud %1 (without smoothing)... "
04404 "[K neighbors=%2]").arg(iter->first).arg(_exportDialog->getMeshNormalKSearch()));
04405 _initProgressDialog->incrementStep();
04406 QApplication::processEvents();
04407
04408 cloudWithNormals = util3d::computeNormals(iter->second, _exportDialog->getMeshNormalKSearch());
04409 }
04410
04411 if(_exportDialog->getMesh())
04412 {
04413 _initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius()));
04414 _initProgressDialog->incrementStep();
04415 QApplication::processEvents();
04416
04417 pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _exportDialog->getMeshGp3Radius());
04418 meshes.insert(std::make_pair(iter->first, mesh));
04419 }
04420 }
04421 }
04422
04423 return true;
04424 }
04425 return false;
04426 }
04427
04428 void MainWindow::resetOdometry()
04429 {
04430 UINFO("reset odometry");
04431 this->post(new OdometryResetEvent());
04432 }
04433
04434 void MainWindow::triggerNewMap()
04435 {
04436 UINFO("trigger a new map");
04437 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
04438 }
04439
04440 void MainWindow::dataRecorder()
04441 {
04442 if(_dataRecorder == 0)
04443 {
04444 QString path = QFileDialog::getSaveFileName(this, tr("Save to..."), _preferencesDialog->getWorkingDirectory()+"/output.db", "RTAB-Map database (*.db)");
04445 if(!path.isEmpty())
04446 {
04447 int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
04448
04449 if(r == QMessageBox::No || r == QMessageBox::Yes)
04450 {
04451 bool recordInRAM = r == QMessageBox::Yes;
04452
04453 _dataRecorder = new DataRecorder(this);
04454 _dataRecorder->setWindowFlags(Qt::Dialog);
04455 _dataRecorder->setAttribute(Qt::WA_DeleteOnClose, true);
04456 _dataRecorder->setWindowTitle(tr("Data recorder (%1)").arg(path));
04457
04458 if(_dataRecorder->init(path, recordInRAM))
04459 {
04460 this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed()));
04461 _dataRecorder->show();
04462 _dataRecorder->registerToEventsManager();
04463 if(_camera)
04464 {
04465 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
04466 }
04467 _ui->actionData_recorder->setEnabled(false);
04468 }
04469 else
04470 {
04471 QMessageBox::warning(this, tr(""), tr("Cannot initialize the data recorder!"));
04472 UERROR("Cannot initialize the data recorder!");
04473 delete _dataRecorder;
04474 _dataRecorder = 0;
04475 }
04476 }
04477 }
04478 }
04479 else
04480 {
04481 UERROR("Only one recorder at the same time.");
04482 }
04483 }
04484
04485 void MainWindow::dataRecorderDestroyed()
04486 {
04487 _ui->actionData_recorder->setEnabled(true);
04488 _dataRecorder = 0;
04489 }
04490
04491
04492
04493 void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode)
04494 {
04495 if(clouds.size() == 1)
04496 {
04497 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+QDir::separator()+"cloud.ply", tr("Point cloud data (*.ply *.pcd)"));
04498 if(!path.isEmpty())
04499 {
04500 if(clouds.begin()->second->size())
04501 {
04502 _initProgressDialog->appendText(tr("Saving the cloud (%1 points)...").arg(clouds.begin()->second->size()));
04503
04504 bool success =false;
04505 if(QFileInfo(path).suffix() == "pcd")
04506 {
04507 success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
04508 }
04509 else if(QFileInfo(path).suffix() == "ply")
04510 {
04511 success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
04512 }
04513 else if(QFileInfo(path).suffix() == "")
04514 {
04515
04516 path += ".ply";
04517 success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
04518 }
04519 else
04520 {
04521 UERROR("Extension not recognized! (%s) Should be one of (*.ply *.pcd).", QFileInfo(path).suffix().toStdString().c_str());
04522 }
04523 if(success)
04524 {
04525 _initProgressDialog->incrementStep();
04526 _initProgressDialog->appendText(tr("Saving the cloud (%1 points)... done.").arg(clouds.begin()->second->size()));
04527
04528 QMessageBox::information(this, tr("Save successful!"), tr("Cloud saved to \"%1\"").arg(path));
04529 }
04530 else
04531 {
04532 QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path));
04533 }
04534 }
04535 else
04536 {
04537 QMessageBox::warning(this, tr("Save failed!"), tr("Cloud is empty..."));
04538 }
04539 }
04540 }
04541 else if(clouds.size())
04542 {
04543 QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply *.pcd)..."), _preferencesDialog->getWorkingDirectory(), 0);
04544 if(!path.isEmpty())
04545 {
04546 bool ok = false;
04547 QStringList items;
04548 items.push_back("ply");
04549 items.push_back("pcd");
04550 QString suffix = QInputDialog::getItem(this, tr("File format"), tr("Which format?"), items, 0, false, &ok);
04551
04552 if(ok)
04553 {
04554 QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "cloud", &ok);
04555
04556 if(ok)
04557 {
04558 for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
04559 {
04560 if(iter->second->size())
04561 {
04562 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud;
04563 transformedCloud = util3d::transformPointCloud<pcl::PointXYZRGB>(iter->second, _currentPosesMap.at(iter->first));
04564
04565 QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
04566 bool success =false;
04567 if(suffix == "pcd")
04568 {
04569 success = pcl::io::savePCDFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
04570 }
04571 else if(suffix == "ply")
04572 {
04573 success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
04574 }
04575 else
04576 {
04577 UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str());
04578 }
04579 if(success)
04580 {
04581 _initProgressDialog->appendText(tr("Saved cloud %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
04582 }
04583 else
04584 {
04585 _initProgressDialog->appendText(tr("Failed saving cloud %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
04586 }
04587 }
04588 else
04589 {
04590 _initProgressDialog->appendText(tr("Cloud %1 is empty!").arg(iter->first));
04591 }
04592 _initProgressDialog->incrementStep();
04593 QApplication::processEvents();
04594 }
04595 }
04596 }
04597 }
04598 }
04599 }
04600
04601 void MainWindow::saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode)
04602 {
04603 if(meshes.size() == 1)
04604 {
04605 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+QDir::separator()+"mesh.ply", tr("Mesh (*.ply)"));
04606 if(!path.isEmpty())
04607 {
04608 if(meshes.begin()->second->polygons.size())
04609 {
04610 _initProgressDialog->appendText(tr("Saving the mesh (%1 polygons)...").arg(meshes.begin()->second->polygons.size()));
04611
04612 bool success =false;
04613 if(QFileInfo(path).suffix() == "ply")
04614 {
04615 if(binaryMode)
04616 {
04617 success = pcl::io::savePLYFileBinary(path.toStdString(), *meshes.begin()->second) == 0;
04618 }
04619 else
04620 {
04621 success = pcl::io::savePLYFile(path.toStdString(), *meshes.begin()->second) == 0;
04622 }
04623 }
04624 else if(QFileInfo(path).suffix() == "")
04625 {
04626
04627 path += ".ply";
04628 if(binaryMode)
04629 {
04630 success = pcl::io::savePLYFileBinary(path.toStdString(), *meshes.begin()->second) == 0;
04631 }
04632 else
04633 {
04634 success = pcl::io::savePLYFile(path.toStdString(), *meshes.begin()->second) == 0;
04635 }
04636 }
04637 else
04638 {
04639 UERROR("Extension not recognized! (%s) Should be (*.ply).", QFileInfo(path).suffix().toStdString().c_str());
04640 }
04641 if(success)
04642 {
04643 _initProgressDialog->incrementStep();
04644 _initProgressDialog->appendText(tr("Saving the mesh (%1 polygons)... done.").arg(meshes.begin()->second->polygons.size()));
04645
04646 QMessageBox::information(this, tr("Save successful!"), tr("Mesh saved to \"%1\"").arg(path));
04647 }
04648 else
04649 {
04650 QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path));
04651 }
04652 }
04653 else
04654 {
04655 QMessageBox::warning(this, tr("Save failed!"), tr("Cloud is empty..."));
04656 }
04657 }
04658 }
04659 else if(meshes.size())
04660 {
04661 QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply)..."), _preferencesDialog->getWorkingDirectory(), 0);
04662 if(!path.isEmpty())
04663 {
04664 bool ok = false;
04665 QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "mesh", &ok);
04666 QString suffix = "ply";
04667
04668 if(ok)
04669 {
04670 for(std::map<int, pcl::PolygonMesh::Ptr>::const_iterator iter=meshes.begin(); iter!=meshes.end(); ++iter)
04671 {
04672 if(iter->second->polygons.size())
04673 {
04674 pcl::PolygonMesh mesh;
04675 mesh.polygons = iter->second->polygons;
04676 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
04677 pcl::fromPCLPointCloud2(iter->second->cloud, *tmp);
04678 tmp = util3d::transformPointCloud<pcl::PointXYZRGB>(tmp, _currentPosesMap.at(iter->first));
04679 pcl::toPCLPointCloud2(*tmp, mesh.cloud);
04680
04681 QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
04682 bool success =false;
04683 if(suffix == "ply")
04684 {
04685 if(binaryMode)
04686 {
04687 success = pcl::io::savePLYFileBinary(pathFile.toStdString(), mesh) == 0;
04688 }
04689 else
04690 {
04691 success = pcl::io::savePLYFile(pathFile.toStdString(), mesh) == 0;
04692 }
04693 }
04694 else
04695 {
04696 UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str());
04697 }
04698 if(success)
04699 {
04700 _initProgressDialog->appendText(tr("Saved mesh %1 (%2 polygons) to %3.")
04701 .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile));
04702 }
04703 else
04704 {
04705 _initProgressDialog->appendText(tr("Failed saving mesh %1 (%2 polygons) to %3.")
04706 .arg(iter->first).arg(iter->second->polygons.size()).arg(pathFile));
04707 }
04708 }
04709 else
04710 {
04711 _initProgressDialog->appendText(tr("Mesh %1 is empty!").arg(iter->first));
04712 }
04713 _initProgressDialog->incrementStep();
04714 QApplication::processEvents();
04715 }
04716 }
04717 }
04718 }
04719 }
04720
04721 void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> & scans, bool binaryMode)
04722 {
04723 if(scans.size() == 1)
04724 {
04725 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+QDir::separator()+"scan.ply", tr("Point cloud data (*.ply *.pcd)"));
04726 if(!path.isEmpty())
04727 {
04728 if(scans.begin()->second->size())
04729 {
04730 _initProgressDialog->appendText(tr("Saving the scan (%1 points)...").arg(scans.begin()->second->size()));
04731
04732 bool success =false;
04733 if(QFileInfo(path).suffix() == "pcd")
04734 {
04735 success = pcl::io::savePCDFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0;
04736 }
04737 else if(QFileInfo(path).suffix() == "ply")
04738 {
04739 success = pcl::io::savePLYFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0;
04740 }
04741 else if(QFileInfo(path).suffix() == "")
04742 {
04743
04744 path += ".ply";
04745 success = pcl::io::savePLYFile(path.toStdString(), *scans.begin()->second, binaryMode) == 0;
04746 }
04747 else
04748 {
04749 UERROR("Extension not recognized! (%s) Should be one of (*.ply *.pcd).", QFileInfo(path).suffix().toStdString().c_str());
04750 }
04751 if(success)
04752 {
04753 _initProgressDialog->incrementStep();
04754 _initProgressDialog->appendText(tr("Saving the scan (%1 points)... done.").arg(scans.begin()->second->size()));
04755
04756 QMessageBox::information(this, tr("Save successful!"), tr("Scan saved to \"%1\"").arg(path));
04757 }
04758 else
04759 {
04760 QMessageBox::warning(this, tr("Save failed!"), tr("Failed to save to \"%1\"").arg(path));
04761 }
04762 }
04763 else
04764 {
04765 QMessageBox::warning(this, tr("Save failed!"), tr("Scan is empty..."));
04766 }
04767 }
04768 }
04769 else if(scans.size())
04770 {
04771 QString path = QFileDialog::getExistingDirectory(this, tr("Save to (*.ply *.pcd)..."), _preferencesDialog->getWorkingDirectory(), 0);
04772 if(!path.isEmpty())
04773 {
04774 bool ok = false;
04775 QStringList items;
04776 items.push_back("ply");
04777 items.push_back("pcd");
04778 QString suffix = QInputDialog::getItem(this, tr("File format"), tr("Which format?"), items, 0, false, &ok);
04779
04780 if(ok)
04781 {
04782 QString prefix = QInputDialog::getText(this, tr("File prefix"), tr("Prefix:"), QLineEdit::Normal, "scan", &ok);
04783
04784 if(ok)
04785 {
04786 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator iter=scans.begin(); iter!=scans.end(); ++iter)
04787 {
04788 if(iter->second->size())
04789 {
04790 pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud;
04791 transformedCloud = util3d::transformPointCloud<pcl::PointXYZ>(iter->second, _currentPosesMap.at(iter->first));
04792
04793 QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
04794 bool success =false;
04795 if(suffix == "pcd")
04796 {
04797 success = pcl::io::savePCDFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
04798 }
04799 else if(suffix == "ply")
04800 {
04801 success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
04802 }
04803 else
04804 {
04805 UFATAL("Extension not recognized! (%s)", suffix.toStdString().c_str());
04806 }
04807 if(success)
04808 {
04809 _initProgressDialog->appendText(tr("Saved scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
04810 }
04811 else
04812 {
04813 _initProgressDialog->appendText(tr("Failed saving scan %1 (%2 points) to %3.").arg(iter->first).arg(iter->second->size()).arg(pathFile));
04814 }
04815 }
04816 else
04817 {
04818 _initProgressDialog->appendText(tr("Scan %1 is empty!").arg(iter->first));
04819 }
04820 _initProgressDialog->incrementStep();
04821 QApplication::processEvents();
04822 }
04823 }
04824 }
04825 }
04826 }
04827 }
04828
04829 pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
04830 int id,
04831 const cv::Mat & rgb,
04832 const cv::Mat & depth,
04833 float fx,
04834 float fy,
04835 float cx,
04836 float cy,
04837 const Transform & localTransform,
04838 const Transform & pose,
04839 float voxelSize,
04840 int decimation,
04841 float maxDepth) const
04842 {
04843 UTimer timer;
04844 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
04845 if(depth.type() == CV_8UC1)
04846 {
04847 cloud = util3d::cloudFromStereoImages(
04848 rgb,
04849 depth,
04850 cx, cy,
04851 fx, fy,
04852 decimation);
04853 }
04854 else
04855 {
04856 cloud = util3d::cloudFromDepthRGB(
04857 rgb,
04858 depth,
04859 cx, cy,
04860 fx, fy,
04861 decimation);
04862 }
04863
04864 if(cloud->size())
04865 {
04866 bool filtered = false;
04867 if(cloud->size() && maxDepth)
04868 {
04869 cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
04870 filtered = true;
04871 }
04872
04873 if(cloud->size() && voxelSize)
04874 {
04875 cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize);
04876 filtered = true;
04877 }
04878
04879 if(cloud->size() && !filtered)
04880 {
04881 cloud = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloud);
04882 }
04883
04884 if(cloud->size())
04885 {
04886 cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose * localTransform);
04887 }
04888 }
04889 UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
04890 return cloud;
04891 }
04892
04893 pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
04894 const std::map<int, Transform> & poses,
04895 float assembledVoxelSize,
04896 bool regenerateClouds,
04897 int regenerateDecimation,
04898 float regenerateVoxelSize,
04899 float regenerateMaxDepth) const
04900 {
04901 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
04902 int i=0;
04903 int count = 0;
04904 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
04905 {
04906 bool inserted = false;
04907 if(!iter->second.isNull())
04908 {
04909 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
04910 if(regenerateClouds)
04911 {
04912 if(_cachedSignatures.contains(iter->first))
04913 {
04914 const Signature & s = _cachedSignatures.find(iter->first).value();
04915 cv::Mat image, depth;
04916 s.uncompressDataConst(&image, &depth, 0);
04917
04918 if(!image.empty() && !depth.empty())
04919 {
04920 cloud = createCloud(iter->first,
04921 image,
04922 depth,
04923 s.getFx(),
04924 s.getFy(),
04925 s.getCx(),
04926 s.getCy(),
04927 s.getLocalTransform(),
04928 iter->second,
04929 regenerateVoxelSize,
04930 regenerateDecimation,
04931 regenerateMaxDepth);
04932 }
04933 else if(s.getWords3().size())
04934 {
04935 cloud->resize(s.getWords3().size());
04936 int oi=0;
04937 for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
04938 {
04939 (*cloud)[oi].x = jter->second.x;
04940 (*cloud)[oi].y = jter->second.y;
04941 (*cloud)[oi].z = jter->second.z;
04942 (*cloud)[oi].r = 255;
04943 (*cloud)[oi].g = 255;
04944 (*cloud)[oi++].b = 255;
04945 }
04946 }
04947 }
04948 else
04949 {
04950 UWARN("Cloud %d not found in cache!", iter->first);
04951 }
04952 }
04953 else if(uContains(_createdClouds, iter->first))
04954 {
04955 cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(_createdClouds.at(iter->first), iter->second);
04956 }
04957
04958 if(cloud->size())
04959 {
04960 *assembledCloud += *cloud;
04961
04962 inserted = true;
04963 ++count;
04964 }
04965 }
04966 else
04967 {
04968 UERROR("transform is null!?");
04969 }
04970
04971 if(inserted)
04972 {
04973 _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
04974
04975 if(count % 100 == 0)
04976 {
04977 if(assembledCloud->size() && assembledVoxelSize)
04978 {
04979 assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
04980 }
04981 }
04982 }
04983 else
04984 {
04985 _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
04986 }
04987 _initProgressDialog->incrementStep();
04988 QApplication::processEvents();
04989 }
04990
04991 if(assembledCloud->size() && assembledVoxelSize)
04992 {
04993 assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
04994 }
04995
04996 return assembledCloud;
04997 }
04998
04999 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
05000 const std::map<int, Transform> & poses,
05001 bool regenerateClouds,
05002 int regenerateDecimation,
05003 float regenerateVoxelSize,
05004 float regenerateMaxDepth) const
05005 {
05006 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
05007 int i=0;
05008 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
05009 {
05010 bool inserted = false;
05011 if(!iter->second.isNull())
05012 {
05013 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
05014 if(regenerateClouds)
05015 {
05016 if(_cachedSignatures.contains(iter->first))
05017 {
05018 const Signature & s = _cachedSignatures.find(iter->first).value();
05019 cv::Mat image, depth;
05020 s.uncompressDataConst(&image, &depth, 0);
05021 if(!image.empty() && !depth.empty())
05022 {
05023 cloud = createCloud(iter->first,
05024 image,
05025 depth,
05026 s.getFx(),
05027 s.getFy(),
05028 s.getCx(),
05029 s.getCy(),
05030 s.getLocalTransform(),
05031 Transform::getIdentity(),
05032 regenerateVoxelSize,
05033 regenerateDecimation,
05034 regenerateMaxDepth);
05035 }
05036 else if(s.getWords3().size())
05037 {
05038 cloud->resize(s.getWords3().size());
05039 int oi=0;
05040 for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
05041 {
05042 (*cloud)[oi].x = jter->second.x;
05043 (*cloud)[oi].y = jter->second.y;
05044 (*cloud)[oi].z = jter->second.z;
05045 (*cloud)[oi].r = 255;
05046 (*cloud)[oi].g = 255;
05047 (*cloud)[oi++].b = 255;
05048 }
05049 }
05050 }
05051 else
05052 {
05053 UERROR("Cloud %d not found in cache!", iter->first);
05054 }
05055 }
05056 else if(uContains(_createdClouds, iter->first))
05057 {
05058 cloud = _createdClouds.at(iter->first);
05059 }
05060
05061 if(cloud->size())
05062 {
05063 clouds.insert(std::make_pair(iter->first, cloud));
05064 inserted = true;
05065 }
05066 }
05067 else
05068 {
05069 UERROR("transform is null!?");
05070 }
05071
05072 if(inserted)
05073 {
05074 _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
05075 }
05076 else
05077 {
05078 _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
05079 }
05080 _initProgressDialog->incrementStep();
05081 QApplication::processEvents();
05082 }
05083
05084 return clouds;
05085 }
05086
05087
05088
05089
05090 void MainWindow::setMonitoringState(bool pauseChecked)
05091 {
05092 this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
05093 }
05094
05095
05096 void MainWindow::changeState(MainWindow::State newState)
05097 {
05098 bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
05099 _ui->actionNew_database->setVisible(!monitoring);
05100 _ui->actionOpen_database->setVisible(!monitoring);
05101 _ui->actionClose_database->setVisible(!monitoring);
05102 _ui->actionEdit_database->setVisible(!monitoring);
05103 _ui->actionStart->setVisible(!monitoring);
05104 _ui->actionStop->setVisible(!monitoring);
05105 _ui->actionDump_the_memory->setVisible(!monitoring);
05106 _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
05107 _ui->actionGenerate_map->setVisible(!monitoring);
05108 _ui->actionGenerate_local_map->setVisible(!monitoring);
05109 _ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
05110 _ui->actionOpen_working_directory->setVisible(!monitoring);
05111 _ui->actionData_recorder->setVisible(!monitoring);
05112 _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
05113 _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
05114 _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
05115 _ui->toolBar->setVisible(!monitoring);
05116 _ui->toolBar->toggleViewAction()->setVisible(!monitoring);
05117 QList<QAction*> actions = _ui->menuTools->actions();
05118 for(int i=0; i<actions.size(); ++i)
05119 {
05120 if(actions.at(i)->isSeparator())
05121 {
05122 actions.at(i)->setVisible(!monitoring);
05123 }
05124 }
05125 actions = _ui->menuFile->actions();
05126 if(actions.size()>=9)
05127 {
05128 if(actions.at(2)->isSeparator())
05129 {
05130 actions.at(2)->setVisible(!monitoring);
05131 }
05132 else
05133 {
05134 UWARN("Menu File separators have not the same order.");
05135 }
05136 if(actions.at(8)->isSeparator())
05137 {
05138 actions.at(8)->setVisible(!monitoring);
05139 }
05140 else
05141 {
05142 UWARN("Menu File separators have not the same order.");
05143 }
05144 }
05145 else
05146 {
05147 UWARN("Menu File separators have not the same order.");
05148 }
05149 actions = _ui->menuProcess->actions();
05150 if(actions.size()>=2)
05151 {
05152 if(actions.at(1)->isSeparator())
05153 {
05154 actions.at(1)->setVisible(!monitoring);
05155 }
05156 else
05157 {
05158 UWARN("Menu File separators have not the same order.");
05159 }
05160 }
05161 else
05162 {
05163 UWARN("Menu File separators have not the same order.");
05164 }
05165
05166
05167 switch (newState)
05168 {
05169 case kIdle:
05170 _ui->actionNew_database->setEnabled(true);
05171 _ui->actionOpen_database->setEnabled(true);
05172 _ui->actionClose_database->setEnabled(false);
05173 _ui->actionEdit_database->setEnabled(true);
05174 _ui->actionStart->setEnabled(false);
05175 _ui->actionPause->setEnabled(false);
05176 _ui->actionPause->setChecked(false);
05177 _ui->actionPause->setToolTip(tr("Pause"));
05178 _ui->actionStop->setEnabled(false);
05179 _ui->actionPause_on_match->setEnabled(true);
05180 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05181 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05182 _ui->actionDump_the_memory->setEnabled(false);
05183 _ui->actionDump_the_prediction_matrix->setEnabled(false);
05184 _ui->actionDelete_memory->setEnabled(false);
05185 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05186 _ui->actionGenerate_map->setEnabled(false);
05187 _ui->actionGenerate_local_map->setEnabled(false);
05188 _ui->actionGenerate_TORO_graph_graph->setEnabled(false);
05189 _ui->actionDownload_all_clouds->setEnabled(false);
05190 _ui->actionDownload_graph->setEnabled(false);
05191 _ui->menuSelect_source->setEnabled(false);
05192 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
05193 _ui->actionTrigger_a_new_map->setEnabled(false);
05194 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
05195 _ui->statusbar->clearMessage();
05196 _state = newState;
05197 _oneSecondTimer->stop();
05198 break;
05199
05200 case kApplicationClosing:
05201 case kClosing:
05202 _ui->actionStart->setEnabled(false);
05203 _ui->actionPause->setEnabled(false);
05204 _ui->actionStop->setEnabled(false);
05205 _state = newState;
05206 break;
05207
05208 case kInitializing:
05209 _ui->actionNew_database->setEnabled(false);
05210 _ui->actionOpen_database->setEnabled(false);
05211 _ui->actionClose_database->setEnabled(false);
05212 _ui->actionEdit_database->setEnabled(false);
05213 _state = newState;
05214 break;
05215
05216 case kInitialized:
05217 _ui->actionNew_database->setEnabled(false);
05218 _ui->actionOpen_database->setEnabled(false);
05219 _ui->actionClose_database->setEnabled(true);
05220 _ui->actionEdit_database->setEnabled(false);
05221 _ui->actionStart->setEnabled(true);
05222 _ui->actionPause->setEnabled(false);
05223 _ui->actionPause->setChecked(false);
05224 _ui->actionPause->setToolTip(tr("Pause"));
05225 _ui->actionStop->setEnabled(false);
05226 _ui->actionPause_on_match->setEnabled(true);
05227 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05228 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05229 _ui->actionDump_the_memory->setEnabled(true);
05230 _ui->actionDump_the_prediction_matrix->setEnabled(true);
05231 _ui->actionDelete_memory->setEnabled(true);
05232 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05233 _ui->actionGenerate_map->setEnabled(true);
05234 _ui->actionGenerate_local_map->setEnabled(true);
05235 _ui->actionGenerate_TORO_graph_graph->setEnabled(true);
05236 _ui->actionDownload_all_clouds->setEnabled(true);
05237 _ui->actionDownload_graph->setEnabled(true);
05238 _ui->menuSelect_source->setEnabled(true);
05239 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
05240 _ui->actionTrigger_a_new_map->setEnabled(true);
05241 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
05242 _ui->statusbar->clearMessage();
05243 _state = newState;
05244 _oneSecondTimer->stop();
05245 break;
05246
05247 case kStartingDetection:
05248 _ui->actionStart->setEnabled(false);
05249 _state = newState;
05250 break;
05251
05252 case kDetecting:
05253 _ui->actionNew_database->setEnabled(false);
05254 _ui->actionOpen_database->setEnabled(false);
05255 _ui->actionClose_database->setEnabled(false);
05256 _ui->actionEdit_database->setEnabled(false);
05257 _ui->actionStart->setEnabled(false);
05258 _ui->actionPause->setEnabled(true);
05259 _ui->actionPause->setChecked(false);
05260 _ui->actionPause->setToolTip(tr("Pause"));
05261 _ui->actionStop->setEnabled(true);
05262 _ui->actionPause_on_match->setEnabled(true);
05263 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05264 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05265 _ui->actionDump_the_memory->setEnabled(false);
05266 _ui->actionDump_the_prediction_matrix->setEnabled(false);
05267 _ui->actionDelete_memory->setEnabled(false);
05268 _ui->actionPost_processing->setEnabled(false);
05269 _ui->actionGenerate_map->setEnabled(false);
05270 _ui->actionGenerate_local_map->setEnabled(false);
05271 _ui->actionGenerate_TORO_graph_graph->setEnabled(false);
05272 _ui->actionDownload_all_clouds->setEnabled(false);
05273 _ui->actionDownload_graph->setEnabled(false);
05274 _ui->menuSelect_source->setEnabled(false);
05275 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
05276 _ui->actionTrigger_a_new_map->setEnabled(true);
05277 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
05278 _ui->statusbar->showMessage(tr("Detecting..."));
05279 _state = newState;
05280 _ui->label_elapsedTime->setText("00:00:00");
05281 _elapsedTime->start();
05282 _oneSecondTimer->start();
05283
05284 _databaseUpdated = true;
05285
05286 if(_camera)
05287 {
05288 _camera->start();
05289 }
05290
05291 if(_dbReader)
05292 {
05293 _dbReader->start();
05294 }
05295 break;
05296
05297 case kPaused:
05298 if(_state == kPaused)
05299 {
05300 _ui->actionPause->setToolTip(tr("Pause"));
05301 _ui->actionPause->setChecked(false);
05302 _ui->statusbar->showMessage(tr("Detecting..."));
05303 _ui->actionDump_the_memory->setEnabled(false);
05304 _ui->actionDump_the_prediction_matrix->setEnabled(false);
05305 _ui->actionDelete_memory->setEnabled(false);
05306 _ui->actionPost_processing->setEnabled(false);
05307 _ui->actionGenerate_map->setEnabled(false);
05308 _ui->actionGenerate_local_map->setEnabled(false);
05309 _ui->actionGenerate_TORO_graph_graph->setEnabled(false);
05310 _ui->actionDownload_all_clouds->setEnabled(false);
05311 _ui->actionDownload_graph->setEnabled(false);
05312 _state = kDetecting;
05313 _elapsedTime->start();
05314 _oneSecondTimer->start();
05315
05316 if(_camera)
05317 {
05318 _camera->start();
05319 }
05320
05321 if(_dbReader)
05322 {
05323 _dbReader->start();
05324 }
05325 }
05326 else if(_state == kDetecting)
05327 {
05328 _ui->actionPause->setToolTip(tr("Continue (shift-click for step-by-step)"));
05329 _ui->actionPause->setChecked(true);
05330 _ui->statusbar->showMessage(tr("Paused..."));
05331 _ui->actionDump_the_memory->setEnabled(true);
05332 _ui->actionDump_the_prediction_matrix->setEnabled(true);
05333 _ui->actionDelete_memory->setEnabled(false);
05334 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05335 _ui->actionGenerate_map->setEnabled(true);
05336 _ui->actionGenerate_local_map->setEnabled(true);
05337 _ui->actionGenerate_TORO_graph_graph->setEnabled(true);
05338 _ui->actionDownload_all_clouds->setEnabled(true);
05339 _ui->actionDownload_graph->setEnabled(true);
05340 _state = kPaused;
05341 _oneSecondTimer->stop();
05342
05343
05344 if(_camera)
05345 {
05346 _camera->join(true);
05347 }
05348
05349 if(_dbReader)
05350 {
05351 _dbReader->join(true);
05352 }
05353 }
05354 break;
05355 case kMonitoring:
05356 _ui->actionPause->setEnabled(true);
05357 _ui->actionPause->setChecked(false);
05358 _ui->actionPause->setToolTip(tr("Pause"));
05359 _ui->actionPause_on_match->setEnabled(true);
05360 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05361 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05362 _ui->actionReset_Odometry->setEnabled(true);
05363 _ui->actionPost_processing->setEnabled(false);
05364 _ui->actionDelete_memory->setEnabled(true);
05365 _ui->actionDownload_all_clouds->setEnabled(true);
05366 _ui->actionDownload_graph->setEnabled(true);
05367 _ui->actionTrigger_a_new_map->setEnabled(true);
05368 _ui->statusbar->showMessage(tr("Monitoring..."));
05369 _state = newState;
05370 _elapsedTime->start();
05371 _oneSecondTimer->start();
05372 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 0));
05373 break;
05374 case kMonitoringPaused:
05375 _ui->actionPause->setToolTip(tr("Continue"));
05376 _ui->actionPause->setChecked(true);
05377 _ui->actionPause->setEnabled(true);
05378 _ui->actionPause_on_match->setEnabled(true);
05379 _ui->actionPause_on_local_loop_detection->setEnabled(true);
05380 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
05381 _ui->actionReset_Odometry->setEnabled(true);
05382 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
05383 _ui->actionDelete_memory->setEnabled(true);
05384 _ui->actionDownload_all_clouds->setEnabled(true);
05385 _ui->actionDownload_graph->setEnabled(true);
05386 _ui->actionTrigger_a_new_map->setEnabled(true);
05387 _ui->statusbar->showMessage(tr("Monitoring paused..."));
05388 _state = newState;
05389 _oneSecondTimer->stop();
05390 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 1));
05391 break;
05392 default:
05393 break;
05394 }
05395
05396 }
05397
05398 }