00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/MainWindow.h"
00029
00030 #include "ui_mainWindow.h"
00031
00032 #include "rtabmap/core/CameraRGB.h"
00033 #include "rtabmap/core/CameraStereo.h"
00034 #include "rtabmap/core/CameraThread.h"
00035 #include "rtabmap/core/IMUThread.h"
00036 #include "rtabmap/core/CameraEvent.h"
00037 #include "rtabmap/core/DBReader.h"
00038 #include "rtabmap/core/Parameters.h"
00039 #include "rtabmap/core/ParamEvent.h"
00040 #include "rtabmap/core/Signature.h"
00041 #include "rtabmap/core/Memory.h"
00042 #include "rtabmap/core/DBDriver.h"
00043 #include "rtabmap/core/RegistrationVis.h"
00044 #include "rtabmap/core/OccupancyGrid.h"
00045 #include "rtabmap/core/GainCompensator.h"
00046 #include "rtabmap/core/Recovery.h"
00047 #include "rtabmap/core/util2d.h"
00048
00049 #include "rtabmap/gui/ImageView.h"
00050 #include "rtabmap/gui/KeypointItem.h"
00051 #include "rtabmap/gui/DataRecorder.h"
00052 #include "rtabmap/gui/DatabaseViewer.h"
00053 #include "rtabmap/gui/PdfPlot.h"
00054 #include "rtabmap/gui/StatsToolBox.h"
00055 #include "rtabmap/gui/ProgressDialog.h"
00056 #include "rtabmap/gui/CloudViewer.h"
00057 #include "rtabmap/gui/LoopClosureViewer.h"
00058 #include "rtabmap/gui/ExportCloudsDialog.h"
00059 #include "rtabmap/gui/ExportBundlerDialog.h"
00060 #include "rtabmap/gui/AboutDialog.h"
00061 #include "rtabmap/gui/PostProcessingDialog.h"
00062 #include "rtabmap/gui/DepthCalibrationDialog.h"
00063 #include "rtabmap/gui/RecoveryState.h"
00064
00065 #include <rtabmap/utilite/UStl.h>
00066 #include <rtabmap/utilite/ULogger.h>
00067 #include <rtabmap/utilite/UEventsManager.h>
00068 #include <rtabmap/utilite/UFile.h>
00069 #include <rtabmap/utilite/UConversion.h>
00070 #include "rtabmap/utilite/UPlot.h"
00071 #include "rtabmap/utilite/UCv2Qt.h"
00072
00073 #include <QtGui/QCloseEvent>
00074 #include <QtGui/QPixmap>
00075 #include <QtCore/QDir>
00076 #include <QtCore/QFile>
00077 #include <QtCore/QTextStream>
00078 #include <QtCore/QFileInfo>
00079 #include <QMessageBox>
00080 #include <QFileDialog>
00081 #include <QGraphicsEllipseItem>
00082 #include <QDockWidget>
00083 #include <QtCore/QBuffer>
00084 #include <QtCore/QTimer>
00085 #include <QtCore/QTime>
00086 #include <QActionGroup>
00087 #include <QtCore/QThread>
00088 #include <QtGui/QDesktopServices>
00089 #include <QtCore/QStringList>
00090 #include <QtCore/QProcess>
00091 #include <QSplashScreen>
00092 #include <QInputDialog>
00093 #include <QToolButton>
00094
00095
00096 #include "rtabmap/core/CameraRGBD.h"
00097 #include "rtabmap/core/Odometry.h"
00098 #include "rtabmap/core/OdometryThread.h"
00099 #include "rtabmap/core/OdometryEvent.h"
00100 #include "rtabmap/core/util3d.h"
00101 #include "rtabmap/core/util3d_transforms.h"
00102 #include "rtabmap/core/util3d_filtering.h"
00103 #include "rtabmap/core/util3d_mapping.h"
00104 #include "rtabmap/core/util3d_surface.h"
00105 #include "rtabmap/core/util3d_registration.h"
00106 #include "rtabmap/core/Optimizer.h"
00107 #include "rtabmap/core/OptimizerCVSBA.h"
00108 #include "rtabmap/core/Graph.h"
00109 #include "rtabmap/core/RegistrationIcp.h"
00110 #include <pcl/visualization/cloud_viewer.h>
00111 #include <pcl/common/transforms.h>
00112 #include <pcl/common/common.h>
00113 #include <pcl/io/pcd_io.h>
00114 #include <pcl/io/ply_io.h>
00115 #include <pcl/filters/filter.h>
00116 #include <pcl/search/kdtree.h>
00117
00118 #ifdef RTABMAP_OCTOMAP
00119 #include <rtabmap/core/OctoMap.h>
00120 #endif
00121
00122 #define LOG_FILE_NAME "LogRtabmap.txt"
00123 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
00124 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
00125 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m"
00126
00127 using namespace rtabmap;
00128
00129 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
00130
00131
00132 namespace rtabmap {
00133
00134 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool showSplashScreen) :
00135 QMainWindow(parent),
00136 _ui(0),
00137 _state(kIdle),
00138 _camera(0),
00139 _odomThread(0),
00140 _imuThread(0),
00141 _preferencesDialog(0),
00142 _aboutDialog(0),
00143 _exportCloudsDialog(0),
00144 _exportBundlerDialog(0),
00145 _dataRecorder(0),
00146 _lastId(0),
00147 _firstStamp(0.0f),
00148 _processingStatistics(false),
00149 _processingDownloadedMap(false),
00150 _recovering(false),
00151 _odometryReceived(false),
00152 _newDatabasePath(""),
00153 _newDatabasePathOutput(""),
00154 _openedDatabasePath(""),
00155 _databaseUpdated(false),
00156 _odomImageShow(true),
00157 _odomImageDepthShow(false),
00158 _savedMaximized(false),
00159 _waypointsIndex(0),
00160 _cachedMemoryUsage(0),
00161 _createdCloudsMemoryUsage(0),
00162 _occupancyGrid(0),
00163 _octomap(0),
00164 _odometryCorrection(Transform::getIdentity()),
00165 _processingOdometry(false),
00166 _oneSecondTimer(0),
00167 _elapsedTime(0),
00168 _posteriorCurve(0),
00169 _likelihoodCurve(0),
00170 _rawLikelihoodCurve(0),
00171 _exportPosesFrame(0),
00172 _autoScreenCaptureOdomSync(false),
00173 _autoScreenCaptureRAM(false),
00174 _autoScreenCapturePNG(false),
00175 _firstCall(true),
00176 _progressCanceled(false)
00177 {
00178 ULogger::registerCurrentThread("MainWindow");
00179 UDEBUG("");
00180
00181 initGuiResource();
00182
00183 QSplashScreen * splash = 0;
00184 if (showSplashScreen)
00185 {
00186 QPixmap pixmap(":images/RTAB-Map.png");
00187 splash = new QSplashScreen(pixmap);
00188 splash->show();
00189 splash->showMessage(tr("Loading..."));
00190 QApplication::processEvents();
00191 }
00192
00193
00194 _aboutDialog = new AboutDialog(this);
00195 _aboutDialog->setObjectName("AboutDialog");
00196 _exportCloudsDialog = new ExportCloudsDialog(this);
00197 _exportCloudsDialog->setObjectName("ExportCloudsDialog");
00198 _exportBundlerDialog = new ExportBundlerDialog(this);
00199 _exportBundlerDialog->setObjectName("ExportBundlerDialog");
00200 _postProcessingDialog = new PostProcessingDialog(this);
00201 _postProcessingDialog->setObjectName("PostProcessingDialog");
00202 _depthCalibrationDialog = new DepthCalibrationDialog(this);
00203 _depthCalibrationDialog->setObjectName("DepthCalibrationDialog");
00204
00205 _ui = new Ui_mainWindow();
00206 UDEBUG("Setup ui...");
00207 _ui->setupUi(this);
00208
00209
00210
00211
00212
00213
00214
00215 _cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
00216 _cloudViewer->setObjectName("widget_cloudViewer");
00217 _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
00218 _loopClosureViewer = new LoopClosureViewer(_ui->layout_loopClosureViewer);
00219 _loopClosureViewer->setObjectName("widget_loopClosureViewer");
00220 _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
00221 UDEBUG("Setup ui... end");
00222
00223 QString title("RTAB-Map[*]");
00224 this->setWindowTitle(title);
00225 this->setWindowIconText(tr("RTAB-Map"));
00226 this->setObjectName("MainWindow");
00227
00228
00229 setDefaultViews();
00230
00231 _ui->widget_mainWindow->setVisible(false);
00232
00233 if(prefDialog)
00234 {
00235 _preferencesDialog = prefDialog;
00236 _preferencesDialog->setParent(this, Qt::Dialog);
00237 }
00238 else
00239 {
00240 _preferencesDialog = new PreferencesDialog(this);
00241 }
00242
00243 _preferencesDialog->setObjectName("PreferencesDialog");
00244 _preferencesDialog->init();
00245
00246
00247 bool statusBarShown = false;
00248 _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown);
00249 _preferencesDialog->loadWindowGeometry(_preferencesDialog);
00250 _preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
00251 _preferencesDialog->loadWindowGeometry(_exportBundlerDialog);
00252 _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
00253 _preferencesDialog->loadWindowGeometry(_depthCalibrationDialog);
00254 _preferencesDialog->loadWindowGeometry(_aboutDialog);
00255 setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
00256
00257 ParametersMap parameters = _preferencesDialog->getAllParameters();
00258 _occupancyGrid = new OccupancyGrid();
00259 #ifdef RTABMAP_OCTOMAP
00260 _octomap = new OctoMap(parameters);
00261 #endif
00262
00263
00264 _oneSecondTimer = new QTimer(this);
00265 _oneSecondTimer->setInterval(1000);
00266 _elapsedTime = new QTime();
00267 _ui->label_elapsedTime->setText("00:00:00");
00268 connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
00269 _logEventTime = new QTime();
00270 _logEventTime->start();
00271
00272
00273 _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
00274 _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
00275 _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
00276 _ui->imageView_odometry->setAlpha(200);
00277 _preferencesDialog->loadWidgetState(_ui->imageView_source);
00278 _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
00279 _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
00280 _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
00281
00282 _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
00283 _ui->posteriorPlot->addCurve(_posteriorCurve, false);
00284 _ui->posteriorPlot->showLegend(false);
00285 _ui->posteriorPlot->setFixedYAxis(0,1);
00286 UPlotCurveThreshold * tc;
00287 tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
00288 connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
00289
00290 _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00291 _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
00292 _ui->likelihoodPlot->showLegend(false);
00293
00294 _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
00295 _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
00296 _ui->rawLikelihoodPlot->showLegend(false);
00297
00298 _progressDialog = new ProgressDialog(this);
00299 _progressDialog->setMinimumWidth(800);
00300 connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancelProgress()));
00301
00302 connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
00303
00304
00305 connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
00306 qRegisterMetaType<MainWindow::State>("MainWindow::State");
00307 connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
00308 connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
00309 qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
00310 connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
00311 qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
00312 connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
00313 connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
00314 connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
00315
00316
00317 _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
00318 _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
00319 _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
00320 _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
00321 _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
00322 _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
00323 _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
00324 _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
00325 _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
00326 _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
00327 _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
00328 _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
00329 _ui->toolBar->setWindowTitle(tr("File toolbar"));
00330 _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
00331 _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
00332 QAction * a = _ui->menuShow_view->addAction("Progress dialog");
00333 a->setCheckable(false);
00334 connect(a, SIGNAL(triggered(bool)), _progressDialog, SLOT(show()));
00335 QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar");
00336 statusBarAction->setCheckable(true);
00337 statusBarAction->setChecked(statusBarShown);
00338 connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool)));
00339
00340
00341 connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
00342 connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
00343 connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00344 connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
00345 connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
00346 connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
00347 connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
00348 connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
00349 connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
00350 connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
00351 connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
00352 connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
00353 connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
00354 connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
00355 connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
00356 connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
00357 connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
00358 connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
00359 connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
00360 connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
00361 connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
00362 connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
00363 connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
00364 connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
00365 connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
00366 connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
00367 connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
00368 connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
00369 connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
00370 connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
00371 connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
00372 connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
00373 connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
00374 connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
00375 connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
00376 connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
00377 connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
00378 connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
00379 connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
00380 connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
00381 connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
00382 connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom()));
00383 connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
00384 connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
00385 connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages()));
00386 connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat()));
00387 connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap()));
00388 connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
00389 connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
00390 connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
00391 connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
00392 connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
00393 connect(_ui->actionDepth_Calibration, SIGNAL(triggered()), this, SLOT(depthCalibration()));
00394
00395 _ui->actionPause->setShortcut(Qt::Key_Space);
00396 _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
00397
00398
00399 this->addAction(_ui->actionSave_GUI_config);
00400 _ui->actionReset_Odometry->setEnabled(false);
00401 _ui->actionPost_processing->setEnabled(false);
00402 _ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
00403
00404 QToolButton* toolButton = new QToolButton(this);
00405 toolButton->setMenu(_ui->menuSelect_source);
00406 toolButton->setPopupMode(QToolButton::InstantPopup);
00407 toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
00408 toolButton->setToolTip("Select sensor driver");
00409 _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
00410
00411 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
00412 connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
00413 #else
00414 _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
00415 #endif
00416
00417
00418 connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
00419 connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
00420 connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00421 connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
00422 connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
00423 connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
00424 connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
00425 connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00426 connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00427 connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
00428 connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
00429 connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
00430 connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
00431 connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
00432 connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
00433 connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
00434 connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
00435 connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
00436 connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
00437 connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
00438 _ui->actionFreenect->setEnabled(CameraFreenect::available());
00439 _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
00440 _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
00441 _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
00442 _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
00443 _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
00444 _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
00445 _ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
00446 _ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
00447 _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
00448 _ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
00449 _ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
00450 _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
00451 _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
00452 _ui->actionStereoZed->setEnabled(CameraStereoZed::available());
00453 this->updateSelectSourceMenu();
00454
00455 connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
00456
00457 QActionGroup * modeGrp = new QActionGroup(this);
00458 modeGrp->addAction(_ui->actionSLAM_mode);
00459 modeGrp->addAction(_ui->actionLocalization_mode);
00460 _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
00461 _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
00462 connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00463 connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
00464 connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
00465
00466
00467 qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
00468 connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
00469 qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
00470 connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
00471
00472 connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
00473 if(prefDialog == 0)
00474 {
00475 connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
00476 }
00477 connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00478 connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00479 connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00480 connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00481 connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00482 connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00483 connect(_exportBundlerDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00484 connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00485 connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00486 connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00487 connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
00488 connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00489 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00490 for(int i=0; i<dockWidgets.size(); ++i)
00491 {
00492 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
00493 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
00494 }
00495 connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00496
00497 _ui->dockWidget_posterior->installEventFilter(this);
00498 _ui->dockWidget_likelihood->installEventFilter(this);
00499 _ui->dockWidget_rawlikelihood->installEventFilter(this);
00500 _ui->dockWidget_statsV2->installEventFilter(this);
00501 _ui->dockWidget_console->installEventFilter(this);
00502 _ui->dockWidget_loopClosureViewer->installEventFilter(this);
00503 _ui->dockWidget_mapVisibility->installEventFilter(this);
00504 _ui->dockWidget_graphViewer->installEventFilter(this);
00505 _ui->dockWidget_odometry->installEventFilter(this);
00506 _ui->dockWidget_cloudViewer->installEventFilter(this);
00507 _ui->dockWidget_imageView->installEventFilter(this);
00508
00509
00510 _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
00511 _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
00512 _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
00513 connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
00514 connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
00515 connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
00516 connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
00517 connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
00518 connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
00519
00520
00521 qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
00522 connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
00523
00524 qRegisterMetaType<rtabmap::CameraInfo>("rtabmap::CameraInfo");
00525 connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo)));
00526
00527 qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
00528 connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool)));
00529
00530 connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
00531
00532
00533 this->changeState(kIdle);
00534 this->applyPrefSettings(PreferencesDialog::kPanelAll);
00535
00536 _ui->statsToolBox->setNewFigureMaxItems(50);
00537 _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00538 _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00539 _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
00540 _cloudViewer->setBackfaceCulling(true, false);
00541 _preferencesDialog->loadWidgetState(_cloudViewer);
00542
00543
00544 _preferencesDialog->loadWidgetState(_exportCloudsDialog);
00545 _preferencesDialog->loadWidgetState(_exportBundlerDialog);
00546 _preferencesDialog->loadWidgetState(_postProcessingDialog);
00547 _preferencesDialog->loadWidgetState(_depthCalibrationDialog);
00548
00549 if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
00550 {
00551 const std::map<std::string, float> & statistics = Statistics::defaultData();
00552 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
00553 {
00554
00555 if(!QString((*iter).first.c_str()).contains("Gt/"))
00556 {
00557 _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
00558 }
00559 }
00560 }
00561
00562 _ui->statsToolBox->updateStat("Planning/From/", false);
00563 _ui->statsToolBox->updateStat("Planning/Time/ms", false);
00564 _ui->statsToolBox->updateStat("Planning/Goal/", false);
00565 _ui->statsToolBox->updateStat("Planning/Poses/", false);
00566 _ui->statsToolBox->updateStat("Planning/Length/m", false);
00567
00568 _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false);
00569 _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false);
00570 _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false);
00571 _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false);
00572 _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false);
00573
00574 _ui->statsToolBox->updateStat("Odometry/ID/", false);
00575 _ui->statsToolBox->updateStat("Odometry/Features/", false);
00576 _ui->statsToolBox->updateStat("Odometry/Matches/", false);
00577 _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
00578 _ui->statsToolBox->updateStat("Odometry/Inliers/", false);
00579 _ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
00580 _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
00581 _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
00582 _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
00583 _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
00584 _ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
00585 _ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
00586 _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
00587 _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
00588 _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
00589 _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
00590 _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
00591 _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
00592 _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
00593 _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
00594 _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
00595 _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
00596 _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
00597 _ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
00598 _ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
00599 _ui->statsToolBox->updateStat("Odometry/Distance/m", false);
00600 _ui->statsToolBox->updateStat("Odometry/Tx/m", false);
00601 _ui->statsToolBox->updateStat("Odometry/Ty/m", false);
00602 _ui->statsToolBox->updateStat("Odometry/Tz/m", false);
00603 _ui->statsToolBox->updateStat("Odometry/Troll/deg", false);
00604 _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", false);
00605 _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", false);
00606 _ui->statsToolBox->updateStat("Odometry/Px/m", false);
00607 _ui->statsToolBox->updateStat("Odometry/Py/m", false);
00608 _ui->statsToolBox->updateStat("Odometry/Pz/m", false);
00609 _ui->statsToolBox->updateStat("Odometry/Proll/deg", false);
00610 _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", false);
00611 _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", false);
00612
00613 _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false);
00614 _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false);
00615 _ui->statsToolBox->updateStat("GUI/Graph Update/ms", false);
00616 #ifdef RTABMAP_OCTOMAP
00617 _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
00618 _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
00619 #endif
00620 _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
00621 _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
00622 _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
00623 _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
00624 _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
00625 #ifdef RTABMAP_OCTOMAP
00626 _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false);
00627 #endif
00628
00629 this->loadFigures();
00630 connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
00631
00632
00633 _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
00634 _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
00635
00636 if (splash)
00637 {
00638 splash->close();
00639 delete splash;
00640 }
00641
00642 this->setFocus();
00643
00644 UDEBUG("");
00645 }
00646
00647 MainWindow::~MainWindow()
00648 {
00649 UDEBUG("");
00650 this->stopDetection();
00651 delete _ui;
00652 delete _elapsedTime;
00653 #ifdef RTABMAP_OCTOMAP
00654 delete _octomap;
00655 #endif
00656 delete _occupancyGrid;
00657 UDEBUG("");
00658 }
00659
00660 void MainWindow::setupMainLayout(bool vertical)
00661 {
00662 if(vertical)
00663 {
00664 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
00665 }
00666 else if(!vertical)
00667 {
00668 qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
00669 }
00670 }
00671
00672 void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
00673 {
00674 UASSERT(cloudViewer);
00675 delete _cloudViewer;
00676 _cloudViewer = cloudViewer;
00677 _cloudViewer->setParent(_ui->layout_cloudViewer);
00678 _cloudViewer->setObjectName("widget_cloudViewer");
00679 _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
00680
00681 _cloudViewer->setBackfaceCulling(true, false);
00682 _preferencesDialog->loadWidgetState(_cloudViewer);
00683
00684 connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
00685 }
00686 void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
00687 {
00688 UASSERT(loopClosureViewer);
00689 delete _loopClosureViewer;
00690 _loopClosureViewer = loopClosureViewer;
00691 _loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
00692 _loopClosureViewer->setObjectName("widget_loopClosureViewer");
00693 _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
00694 }
00695
00696 void MainWindow::closeEvent(QCloseEvent* event)
00697 {
00698
00699
00700
00701
00702
00703
00704
00705
00706 UDEBUG("");
00707 bool processStopped = true;
00708 if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
00709 {
00710 this->stopDetection();
00711 if(_state == kInitialized)
00712 {
00713 if(this->closeDatabase())
00714 {
00715 this->changeState(kApplicationClosing);
00716 }
00717 }
00718 if(_state != kIdle)
00719 {
00720 processStopped = false;
00721 }
00722 }
00723
00724 if(processStopped)
00725 {
00726
00727 bool save = false;
00728 if(this->isWindowModified())
00729 {
00730 QMessageBox::Button b=QMessageBox::question(this,
00731 tr("RTAB-Map"),
00732 tr("There are unsaved changed settings. Save them?"),
00733 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
00734 if(b == QMessageBox::Save)
00735 {
00736 save = true;
00737 }
00738 else if(b != QMessageBox::Discard)
00739 {
00740 event->ignore();
00741 return;
00742 }
00743 }
00744
00745 if(save)
00746 {
00747 saveConfigGUI();
00748 }
00749
00750 _ui->statsToolBox->closeFigures();
00751
00752 _ui->dockWidget_imageView->close();
00753 _ui->dockWidget_likelihood->close();
00754 _ui->dockWidget_rawlikelihood->close();
00755 _ui->dockWidget_posterior->close();
00756 _ui->dockWidget_statsV2->close();
00757 _ui->dockWidget_console->close();
00758 _ui->dockWidget_cloudViewer->close();
00759 _ui->dockWidget_loopClosureViewer->close();
00760 _ui->dockWidget_mapVisibility->close();
00761 _ui->dockWidget_graphViewer->close();
00762 _ui->dockWidget_odometry->close();
00763
00764 if(_camera)
00765 {
00766 UERROR("Camera must be already deleted here!");
00767 delete _camera;
00768 _camera = 0;
00769 if(_imuThread)
00770 {
00771 delete _imuThread;
00772 _imuThread = 0;
00773 }
00774 }
00775 if(_odomThread)
00776 {
00777 UERROR("OdomThread must be already deleted here!");
00778 delete _odomThread;
00779 _odomThread = 0;
00780 }
00781 event->accept();
00782 }
00783 else
00784 {
00785 event->ignore();
00786 }
00787 UDEBUG("");
00788 }
00789
00790 bool MainWindow::handleEvent(UEvent* anEvent)
00791 {
00792 if(anEvent->getClassName().compare("RtabmapEvent") == 0)
00793 {
00794 RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
00795 Statistics stats = rtabmapEvent->getStats();
00796 int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
00797 int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
00798 bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
00799 float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
00800 if((stats.loopClosureId() > 0 &&
00801 _ui->actionPause_on_match->isChecked())
00802 ||
00803 (stats.loopClosureId() == 0 &&
00804 highestHypothesisId > 0 &&
00805 highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
00806 _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
00807 rejectedHyp)
00808 ||
00809 (proximityClosureId > 0 &&
00810 _ui->actionPause_on_local_loop_detection->isChecked()))
00811 {
00812 if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
00813 {
00814 if(_preferencesDialog->beepOnPause())
00815 {
00816 QMetaObject::invokeMethod(this, "beep");
00817 }
00818 this->pauseDetection();
00819 }
00820 }
00821
00822 if(!_processingDownloadedMap)
00823 {
00824 _processingStatistics = true;
00825 Q_EMIT statsReceived(stats);
00826 }
00827 }
00828 else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
00829 {
00830 if(!_recovering)
00831 {
00832 RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
00833 Q_EMIT rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
00834 }
00835 }
00836 else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
00837 {
00838 RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
00839 Q_EMIT rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
00840 }
00841 else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
00842 {
00843 RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
00844 Q_EMIT rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
00845 }
00846 else if(anEvent->getClassName().compare("RtabmapLabelErrorEvent") == 0)
00847 {
00848 RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
00849 Q_EMIT rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
00850 }
00851 else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
00852 {
00853 Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode());
00854 }
00855 else if(anEvent->getClassName().compare("CameraEvent") == 0)
00856 {
00857 CameraEvent * cameraEvent = (CameraEvent*)anEvent;
00858 if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
00859 {
00860 if(_preferencesDialog->beepOnPause())
00861 {
00862 QMetaObject::invokeMethod(this, "beep");
00863 }
00864 Q_EMIT noMoreImagesReceived();
00865 }
00866 else
00867 {
00868 Q_EMIT cameraInfoReceived(cameraEvent->info());
00869 if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
00870 {
00871 OdometryInfo odomInfo;
00872 odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
00873 if (!_processingOdometry && !_processingStatistics)
00874 {
00875 _processingOdometry = true;
00876 OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo);
00877 Q_EMIT odometryReceived(tmp, false);
00878 }
00879 else
00880 {
00881
00882 }
00883 }
00884 }
00885 }
00886 else if(anEvent->getClassName().compare("OdometryEvent") == 0)
00887 {
00888 OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
00889 if(!_processingOdometry && !_processingStatistics)
00890 {
00891 _processingOdometry = true;
00892 Q_EMIT odometryReceived(*odomEvent, false);
00893 }
00894 else
00895 {
00896
00897 SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
00898 data.setCameraModels(odomEvent->data().cameraModels());
00899 data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
00900 data.setGroundTruth(odomEvent->data().groundTruth());
00901 OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
00902 Q_EMIT odometryReceived(tmp, true);
00903 }
00904 }
00905 else if(anEvent->getClassName().compare("ULogEvent") == 0)
00906 {
00907 ULogEvent * logEvent = (ULogEvent*)anEvent;
00908 if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
00909 {
00910 QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
00911
00912 if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
00913 {
00914 _logEventTime->start();
00915 if(_preferencesDialog->beepOnPause())
00916 {
00917 QMetaObject::invokeMethod(this, "beep");
00918 }
00919 pauseDetection();
00920 }
00921 }
00922 }
00923 return false;
00924 }
00925
00926 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
00927 {
00928 if(_firstStamp == 0.0)
00929 {
00930 _firstStamp = info.stamp;
00931 }
00932 _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00933 _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00934 _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00935 _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00936 _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00937 _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00938 _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00939 _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00940 _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
00941
00942 Q_EMIT(cameraInfoProcessed());
00943 }
00944
00945 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
00946 {
00947 if(_firstStamp == 0.0)
00948 {
00949 _firstStamp = odom.data().stamp();
00950 }
00951
00952 UDEBUG("");
00953 _processingOdometry = true;
00954 UTimer time;
00955
00956
00957
00958 if(_ui->imageView_odometry->toolTip().isEmpty())
00959 {
00960 _ui->imageView_odometry->setToolTip(
00961 "Background Color Code:\n"
00962 " Dark Red = Odometry Lost\n"
00963 " Dark Yellow = Low Inliers\n"
00964 "Feature Color code:\n"
00965 " Green = Inliers\n"
00966 " Yellow = Not matched features from previous frame(s)\n"
00967 " Red = Outliers");
00968 }
00969
00970 Transform pose = odom.pose();
00971 bool lost = false;
00972 bool lostStateChanged = false;
00973
00974 if(pose.isNull())
00975 {
00976 UDEBUG("odom lost");
00977 lostStateChanged = _cloudViewer->getBackgroundColor() != Qt::darkRed;
00978 _cloudViewer->setBackgroundColor(Qt::darkRed);
00979 _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
00980
00981 pose = _lastOdomPose;
00982 lost = true;
00983 }
00984 else if(odom.info().reg.inliers>0 &&
00985 _preferencesDialog->getOdomQualityWarnThr() &&
00986 odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
00987 {
00988 UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
00989 lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
00990 _cloudViewer->setBackgroundColor(Qt::darkYellow);
00991 _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
00992 }
00993 else
00994 {
00995 UDEBUG("odom ok");
00996 lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
00997 _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
00998 _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
00999 }
01000
01001 if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
01002 {
01003 _lastOdomPose = pose;
01004 }
01005
01006 if(_ui->dockWidget_cloudViewer->isVisible())
01007 {
01008 bool cloudUpdated = false;
01009 bool scanUpdated = false;
01010 bool featuresUpdated = false;
01011 if(!pose.isNull())
01012 {
01013
01014 if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
01015 odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
01016 !odom.data().depthOrRightRaw().empty() &&
01017 (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
01018 _preferencesDialog->isCloudsShown(1))
01019 {
01020 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01021 pcl::IndicesPtr indices(new std::vector<int>);
01022 cloud = util3d::cloudRGBFromSensorData(odom.data(),
01023 _preferencesDialog->getCloudDecimation(1),
01024 _preferencesDialog->getCloudMaxDepth(1),
01025 _preferencesDialog->getCloudMinDepth(1),
01026 indices.get(),
01027 _preferencesDialog->getAllParameters(),
01028 _preferencesDialog->getCloudRoiRatios(1));
01029 if(indices->size())
01030 {
01031 cloud = util3d::transformPointCloud(cloud, pose);
01032
01033 if(_preferencesDialog->isCloudMeshing())
01034 {
01035
01036 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
01037 output = util3d::extractIndices(cloud, indices, false, true);
01038
01039
01040 Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
01041 if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
01042 {
01043 viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
01044 viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
01045 viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
01046 }
01047 else if(!odom.data().stereoCameraModel().localTransform().isNull())
01048 {
01049 viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
01050 viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
01051 viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
01052 }
01053 std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
01054 output,
01055 _preferencesDialog->getCloudMeshingAngle(),
01056 _preferencesDialog->isCloudMeshingQuad(),
01057 _preferencesDialog->getCloudMeshingTriangleSize(),
01058 Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
01059 if(polygons.size())
01060 {
01061 if(_preferencesDialog->isCloudMeshingTexture() && !odom.data().imageRaw().empty())
01062 {
01063 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
01064 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
01065 textureMesh->tex_polygons.push_back(polygons);
01066 int w = cloud->width;
01067 int h = cloud->height;
01068 UASSERT(w > 1 && h > 1);
01069 textureMesh->tex_coordinates.resize(1);
01070 int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
01071 textureMesh->tex_coordinates[0].resize(nPoints);
01072 for(int i=0; i<nPoints; ++i)
01073 {
01074
01075 textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
01076 float(i % w) / float(w),
01077 float(h - i / w) / float(h));
01078 }
01079
01080 pcl::TexMaterial mesh_material;
01081 mesh_material.tex_d = 1.0f;
01082 mesh_material.tex_Ns = 75.0f;
01083 mesh_material.tex_illum = 1;
01084
01085 mesh_material.tex_name = "material_odom";
01086 mesh_material.tex_file = "";
01087 textureMesh->tex_materials.push_back(mesh_material);
01088
01089 if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, odom.data().imageRaw(), _odometryCorrection))
01090 {
01091 UERROR("Adding cloudOdom to viewer failed!");
01092 }
01093 }
01094 else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
01095 {
01096 UERROR("Adding cloudOdom to viewer failed!");
01097 }
01098 }
01099 }
01100 else
01101 {
01102 if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
01103 {
01104 UERROR("Adding cloudOdom to viewer failed!");
01105 }
01106 }
01107 _cloudViewer->setCloudVisibility("cloudOdom", true);
01108 _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
01109 _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
01110 _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
01111
01112 cloudUpdated = true;
01113 }
01114 }
01115
01116 if(_preferencesDialog->isScansShown(1))
01117 {
01118
01119 if(!odom.info().localScanMap.isEmpty())
01120 {
01121 if(!lost)
01122 {
01123 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
01124 cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
01125 bool scanAdded = _cloudViewer->getAddedClouds().contains("scanMapOdom");
01126 if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
01127 {
01128 UERROR("Adding scanMapOdom to viewer failed!");
01129 }
01130 else
01131 {
01132 _cloudViewer->setCloudVisibility("scanMapOdom", true);
01133 _cloudViewer->setCloudColorIndex("scanMapOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
01134 _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
01135 _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
01136 }
01137 }
01138 scanUpdated = true;
01139 }
01140
01141 if(!odom.data().laserScanRaw().isEmpty())
01142 {
01143 LaserScan scan = odom.data().laserScanRaw();
01144
01145 if(_preferencesDialog->getDownsamplingStepScan(1) > 1 ||
01146 _preferencesDialog->getScanMaxRange(1) > 0.0f ||
01147 _preferencesDialog->getScanMinRange(1) > 0.0f)
01148 {
01149 scan = util3d::commonFiltering(scan,
01150 _preferencesDialog->getDownsamplingStepScan(1),
01151 _preferencesDialog->getScanMinRange(1),
01152 _preferencesDialog->getScanMaxRange(1));
01153 }
01154
01155 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
01156 cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
01157 if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
01158 {
01159 cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
01160 }
01161
01162 bool scanAdded = !_cloudViewer->getAddedClouds().contains("scanOdom");
01163 if(!_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
01164 {
01165 UERROR("Adding scanOdom to viewer failed!");
01166 }
01167 else
01168 {
01169 _cloudViewer->setCloudVisibility("scanOdom", true);
01170 _cloudViewer->setCloudColorIndex("scanOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
01171 _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
01172 _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
01173 scanUpdated = true;
01174 }
01175 }
01176 }
01177
01178
01179 if(_preferencesDialog->isFeaturesShown(1) && !odom.info().localMap.empty())
01180 {
01181 if(!lost)
01182 {
01183 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01184 cloud->resize(odom.info().localMap.size());
01185 int i=0;
01186 for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
01187 {
01188
01189 if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
01190 {
01191 (*cloud)[i].x = iter->second.x;
01192 (*cloud)[i].y = iter->second.y;
01193 (*cloud)[i].z = iter->second.z;
01194
01195
01196 bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
01197 (*cloud)[i].r = inlier?0:255;
01198 (*cloud)[i].g = 255;
01199 (*cloud)[i].b = 0;
01200 if(!_preferencesDialog->isOdomOnlyInliersShown() || inlier)
01201 {
01202 ++i;
01203 }
01204 }
01205 }
01206 cloud->resize(i);
01207
01208 _cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
01209 _cloudViewer->setCloudVisibility("featuresOdom", true);
01210 _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
01211 }
01212 featuresUpdated = true;
01213 }
01214
01215 if(_preferencesDialog->isFrustumsShown(1))
01216 {
01217 QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
01218 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
01219 {
01220 std::list<std::string> splitted = uSplitNumChar(iter.key());
01221 if(splitted.size() == 2)
01222 {
01223 int id = std::atoi(splitted.back().c_str());
01224 if(splitted.front().compare("f_odom_") == 0 &&
01225 odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
01226 {
01227 _cloudViewer->removeFrustum(iter.key());
01228 }
01229 }
01230 }
01231
01232 for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
01233 {
01234 std::string frustumId = uFormat("f_odom_%d", iter->first);
01235 if(_cloudViewer->getAddedFrustums().contains(frustumId))
01236 {
01237 _cloudViewer->updateFrustumPose(frustumId, _odometryCorrection*iter->second);
01238 }
01239 else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
01240 {
01241 const CameraModel & model = odom.info().localBundleModels.at(iter->first);
01242 Transform t = model.localTransform();
01243 if(!t.isNull())
01244 {
01245 QColor color = Qt::yellow;
01246 _cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color);
01247 }
01248 }
01249 }
01250 }
01251 }
01252 if(!dataIgnored)
01253 {
01254 if(!cloudUpdated && _cloudViewer->getAddedClouds().contains("cloudOdom"))
01255 {
01256 _cloudViewer->setCloudVisibility("cloudOdom", false);
01257 }
01258 if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanOdom"))
01259 {
01260 _cloudViewer->setCloudVisibility("scanOdom", false);
01261 }
01262 if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanMapOdom"))
01263 {
01264 _cloudViewer->setCloudVisibility("scanMapOdom", false);
01265 }
01266 if(!featuresUpdated && _cloudViewer->getAddedClouds().contains("featuresOdom"))
01267 {
01268 _cloudViewer->setCloudVisibility("featuresOdom", false);
01269 }
01270 }
01271 }
01272
01273 if(!odom.pose().isNull())
01274 {
01275 _odometryReceived = true;
01276
01277 _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
01278
01279 if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
01280 {
01281 _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
01282 }
01283 else if(!odom.data().stereoCameraModel().localTransform().isNull())
01284 {
01285 _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
01286 }
01287
01288 }
01289 _cloudViewer->update();
01290
01291 if(_ui->graphicsView_graphView->isVisible())
01292 {
01293 if(!pose.isNull() && !odom.pose().isNull())
01294 {
01295 _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
01296 _ui->graphicsView_graphView->update();
01297 }
01298 }
01299
01300 if(_ui->dockWidget_odometry->isVisible() &&
01301 !odom.data().imageRaw().empty())
01302 {
01303 if(_ui->imageView_odometry->isFeaturesShown())
01304 {
01305 if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
01306 {
01307 if(_preferencesDialog->isOdomOnlyInliersShown())
01308 {
01309 std::multimap<int, cv::KeyPoint> kpInliers;
01310 for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
01311 {
01312 kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
01313 }
01314 _ui->imageView_odometry->setFeatures(
01315 kpInliers,
01316 odom.data().depthRaw(),
01317 Qt::green);
01318 }
01319 else
01320 {
01321 _ui->imageView_odometry->setFeatures(
01322 odom.info().words,
01323 odom.data().depthRaw(),
01324 Qt::yellow);
01325 }
01326 }
01327 else if(odom.info().type == (int)Odometry::kTypeF2F ||
01328 odom.info().type == (int)Odometry::kTypeViso2 ||
01329 odom.info().type == (int)Odometry::kTypeFovis ||
01330 odom.info().type == (int)Odometry::kTypeMSCKF)
01331 {
01332 std::vector<cv::KeyPoint> kpts;
01333 cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
01334 _ui->imageView_odometry->setFeatures(
01335 kpts,
01336 odom.data().depthRaw(),
01337 Qt::red);
01338 }
01339 }
01340
01341
01342 bool monoInitialization = false;
01343 if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F)
01344 {
01345 monoInitialization = true;
01346 }
01347
01348 _ui->imageView_odometry->clearLines();
01349 if(lost && !monoInitialization)
01350 {
01351 if(lostStateChanged)
01352 {
01353
01354 _odomImageShow = _ui->imageView_odometry->isImageShown();
01355 _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
01356 }
01357 _ui->imageView_odometry->setImageDepth(odom.data().imageRaw());
01358 _ui->imageView_odometry->setImageShown(true);
01359 _ui->imageView_odometry->setImageDepthShown(true);
01360 }
01361 else
01362 {
01363 if(lostStateChanged)
01364 {
01365
01366 _ui->imageView_odometry->setImageShown(_odomImageShow);
01367 _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
01368 }
01369
01370 _ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
01371 if(_ui->imageView_odometry->isImageDepthShown() && !odom.data().depthOrRightRaw().empty())
01372 {
01373 _ui->imageView_odometry->setImageDepth(odom.data().depthOrRightRaw());
01374 }
01375
01376 if( odom.info().type == (int)Odometry::kTypeF2M ||
01377 odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
01378 odom.info().type == (int)Odometry::kTypeMSCKF)
01379 {
01380 if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
01381 {
01382 for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
01383 {
01384 _ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red);
01385 }
01386 for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
01387 {
01388 _ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green);
01389 }
01390 }
01391 }
01392 if((odom.info().type == (int)Odometry::kTypeF2F ||
01393 odom.info().type == (int)Odometry::kTypeViso2 ||
01394 odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size())
01395 {
01396 if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
01397 {
01398
01399 UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
01400 std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
01401 for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
01402 {
01403 if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
01404 {
01405 _ui->imageView_odometry->setFeatureColor(i, Qt::green);
01406 }
01407 if(_ui->imageView_odometry->isLinesShown())
01408 {
01409 _ui->imageView_odometry->addLine(
01410 odom.info().newCorners[i].x,
01411 odom.info().newCorners[i].y,
01412 odom.info().refCorners[i].x,
01413 odom.info().refCorners[i].y,
01414 inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
01415 }
01416 }
01417 }
01418 }
01419 }
01420 if(!odom.data().imageRaw().empty())
01421 {
01422 _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
01423 }
01424
01425 _ui->imageView_odometry->update();
01426 }
01427
01428 if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
01429 {
01430 this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
01431 }
01432
01433
01434 _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
01435 _ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
01436 _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
01437 _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
01438 _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
01439 _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
01440 _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
01441 _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
01442 _ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
01443 _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
01444 _ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
01445 _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
01446 _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
01447 _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
01448 _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
01449 _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
01450 _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
01451 _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
01452 _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
01453 _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
01454 _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
01455 _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
01456 _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
01457
01458 float x=0.0f,y,z, roll,pitch,yaw;
01459 if(!odom.info().transform.isNull())
01460 {
01461 odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01462 _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
01463 _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
01464 _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
01465 _ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01466 _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01467 _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01468 }
01469
01470 if(!odom.info().transformFiltered.isNull())
01471 {
01472 odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01473 _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
01474 _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
01475 _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
01476 _ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01477 _ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01478 _ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01479 }
01480 if(odom.info().interval > 0)
01481 {
01482 _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
01483 _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
01484 }
01485
01486 if(!odom.info().transformGroundTruth.isNull())
01487 {
01488 odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01489 _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
01490 _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
01491 _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
01492 _ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01493 _ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01494 _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01495 if(odom.info().interval > 0)
01496 {
01497 _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
01498 }
01499 }
01500
01501
01502 if(!odom.pose().isNull())
01503 {
01504 odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01505 _ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
01506 _ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
01507 _ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
01508 _ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01509 _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01510 _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01511 }
01512 if(!odom.data().groundTruth().isNull())
01513 {
01514 odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
01515 _ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
01516 _ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
01517 _ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
01518 _ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01519 _ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01520 _ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
01521 }
01522
01523 if(odom.info().distanceTravelled > 0)
01524 {
01525 _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
01526 }
01527
01528 _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
01529 _processingOdometry = false;
01530
01531 Q_EMIT(odometryProcessed());
01532 }
01533
01534 void MainWindow::processStats(const rtabmap::Statistics & stat)
01535 {
01536 _processingStatistics = true;
01537 ULOGGER_DEBUG("");
01538 QTime time, totalTime;
01539 time.start();
01540 totalTime.start();
01541
01542
01543 if(_firstStamp == 0.0)
01544 {
01545 _firstStamp = stat.stamp();
01546 }
01547
01548 int refMapId = -1, loopMapId = -1;
01549 if(uContains(stat.getSignatures(), stat.refImageId()))
01550 {
01551 refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
01552 }
01553 int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
01554 int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
01555 if(_cachedSignatures.contains(loopId))
01556 {
01557 loopMapId = _cachedSignatures.value(loopId).mapId();
01558 }
01559
01560 _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
01561
01562 if(stat.extended())
01563 {
01564 float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
01565 if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
01566 {
01567 UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
01568 }
01569
01570 UDEBUG("");
01571 bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
01572
01573 bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
01574
01575 bool fastMovement = (bool)uValue(stat.data(), Statistics::kMemoryFast_movement(), 0.0f);
01576
01577
01578 Signature signature;
01579 if(uContains(stat.getSignatures(), stat.refImageId()))
01580 {
01581 signature = stat.getSignatures().at(stat.refImageId());
01582 signature.sensorData().uncompressData();
01583
01584 if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
01585 signature.getWeight()>=0)
01586 {
01587 if(smallMovement || fastMovement)
01588 {
01589 _cachedSignatures.insert(-1, signature);
01590 }
01591 else
01592 {
01593 _cachedSignatures.insert(signature.id(), signature);
01594 _cachedMemoryUsage += signature.sensorData().getMemoryUsed();
01595 unsigned int count = 0;
01596 for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().upper_bound(-1); jter!=signature.getWords3().end(); ++jter)
01597 {
01598 if(util3d::isFinite(jter->second))
01599 {
01600 ++count;
01601 }
01602 }
01603 _cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
01604 }
01605 }
01606 }
01607
01608
01609 if(signature.getWeight() >= 0 &&
01610 (!signature.sensorData().imageRaw().empty() || signature.getWords().size()))
01611 {
01612 _ui->imageView_source->clear();
01613 _ui->imageView_loopClosure->clear();
01614
01615 _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
01616 _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
01617
01618 _ui->label_matchId->clear();
01619
01620
01621 int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
01622 bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
01623 int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
01624 bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
01625 _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
01626
01627 if(rehearsalMerged > 0)
01628 {
01629 _ui->imageView_source->setBackgroundColor(Qt::blue);
01630 }
01631 else if(proximityTimeDetections > 0)
01632 {
01633 _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
01634 }
01635 else if(scanMatchingSuccess)
01636 {
01637 _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
01638 }
01639 else if(rehearsedSimilarity)
01640 {
01641 _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
01642 }
01643 else if(smallMovement)
01644 {
01645 _ui->imageView_source->setBackgroundColor(Qt::gray);
01646 }
01647 else if(fastMovement)
01648 {
01649 _ui->imageView_source->setBackgroundColor(Qt::magenta);
01650 }
01651
01652 if(_ui->label_refId->toolTip().isEmpty())
01653 {
01654 _ui->label_refId->setToolTip(
01655 "Background Color Code:\n"
01656 " Blue = Weight Update Merged\n"
01657 " Dark Blue = Weight Update\n"
01658 " Dark Yellow = Proximity Detection in Time\n"
01659 " Dark Cyan = Neighbor Link Refined\n"
01660 " Gray = Small Movement\n"
01661 " Magenta = Fast Movement\n"
01662 "Feature Color code:\n"
01663 " Green = New\n"
01664 " Yellow = New but Not Unique\n"
01665 " Red = In Vocabulary\n"
01666 " Blue = In Vocabulary and in Previous Signature\n"
01667 " Pink = In Vocabulary and in Loop Closure Signature\n"
01668 " Gray = Not Quantized to Vocabulary");
01669 }
01670
01671 if(_ui->label_matchId->toolTip().isEmpty())
01672 {
01673 _ui->label_matchId->setToolTip(
01674 "Background Color Code:\n"
01675 " Green = Accepted Loop Closure Detection\n"
01676 " Red = Rejected Loop Closure Detection\n"
01677 " Yellow = Proximity Detection in Space\n"
01678 "Feature Color code:\n"
01679 " Red = In Vocabulary\n"
01680 " Pink = In Vocabulary and in Loop Closure Signature\n"
01681 " Gray = Not Quantized to Vocabulary");
01682 }
01683
01684 UDEBUG("time= %d ms", time.restart());
01685
01686 int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
01687 float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
01688 int matchId = 0;
01689 Signature loopSignature;
01690 int shownLoopId = 0;
01691 if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
01692 {
01693 bool show = true;
01694 if(stat.loopClosureId() > 0)
01695 {
01696 _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
01697 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
01698 if(highestHypothesisIsSaved)
01699 {
01700 _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
01701 }
01702 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
01703 matchId = stat.loopClosureId();
01704 }
01705 else if(stat.proximityDetectionId())
01706 {
01707 _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
01708 _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
01709 matchId = stat.proximityDetectionId();
01710 }
01711 else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
01712 {
01713 show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
01714 if(show)
01715 {
01716 _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
01717 _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
01718 _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
01719 }
01720 }
01721 else
01722 {
01723 show = _preferencesDialog->imageHighestHypShown();
01724 if(show)
01725 {
01726 _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
01727 }
01728 }
01729
01730 if(show)
01731 {
01732 shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
01733 QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
01734 if(iter != _cachedSignatures.end())
01735 {
01736
01737 loopSignature = iter.value();
01738 loopSignature.sensorData().uncompressData();
01739 }
01740 }
01741 }
01742 _refIds.push_back(stat.refImageId());
01743 _loopClosureIds.push_back(matchId);
01744 if(matchId > 0)
01745 {
01746 _cachedLocalizationsCount[matchId] += 1.0f;
01747 }
01748
01749
01750 {
01751 UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
01752 UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
01753 qimageThread.start();
01754 qimageLoopThread.start();
01755 qimageThread.join();
01756 qimageLoopThread.join();
01757 QImage img = qimageThread.getQImage();
01758 QImage lcImg = qimageLoopThread.getQImage();
01759 UDEBUG("time= %d ms", time.restart());
01760
01761 if(!img.isNull())
01762 {
01763 _ui->imageView_source->setImage(img);
01764 }
01765 if(!signature.sensorData().depthOrRightRaw().empty())
01766 {
01767 _ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
01768 }
01769 if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
01770 {
01771 QRect sceneRect;
01772 if(signature.sensorData().cameraModels().size())
01773 {
01774 for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
01775 {
01776 sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
01777 sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
01778 }
01779 }
01780 else if(signature.sensorData().stereoCameraModel().isValidForProjection())
01781 {
01782 sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
01783 }
01784 if(sceneRect.isValid())
01785 {
01786 _ui->imageView_source->setSceneRect(sceneRect);
01787 }
01788 }
01789 if(!lcImg.isNull())
01790 {
01791 _ui->imageView_loopClosure->setImage(lcImg);
01792 }
01793 if(!loopSignature.sensorData().depthOrRightRaw().empty())
01794 {
01795 _ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
01796 }
01797 if(_ui->imageView_loopClosure->sceneRect().isNull())
01798 {
01799 _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
01800 }
01801 }
01802
01803 UDEBUG("time= %d ms", time.restart());
01804
01805
01806 this->drawKeypoints(signature.getWords(), loopSignature.getWords());
01807
01808 UDEBUG("time= %d ms", time.restart());
01809
01810 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
01811 _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
01812
01813
01814 if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
01815 !stat.loopClosureTransform().isNull() &&
01816 !loopSignature.sensorData().imageRaw().empty())
01817 {
01818
01819 Transform loopClosureTransform = stat.loopClosureTransform();
01820 signature.setPose(loopClosureTransform);
01821 _loopClosureViewer->setData(loopSignature, signature);
01822 if(_ui->dockWidget_loopClosureViewer->isVisible())
01823 {
01824 UTimer loopTimer;
01825 _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
01826 UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
01827 _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
01828 }
01829
01830 UDEBUG("time= %d ms", time.restart());
01831 }
01832 }
01833
01834
01835 if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
01836 {
01837 UDEBUG("");
01838 _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
01839
01840 ULOGGER_DEBUG("");
01841
01842 float value;
01843 value = float(_preferencesDialog->getLoopThr());
01844 Q_EMIT(loopClosureThrChanged(value));
01845 }
01846 if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
01847 {
01848 _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
01849 }
01850 if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
01851 {
01852 _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
01853 }
01854
01855
01856 const std::map<std::string, float> & statistics = stat.data();
01857 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
01858 {
01859
01860 _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
01861 }
01862
01863 UDEBUG("time= %d ms", time.restart());
01864
01865
01866
01867
01868
01869 if(stat.poses().size())
01870 {
01871
01872 std::map<int, int> mapIds;
01873 std::map<int, Transform> groundTruth;
01874 std::map<int, std::string> labels;
01875 for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
01876 {
01877 mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
01878 if(!iter->second.getGroundTruthPose().isNull())
01879 {
01880 groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
01881 }
01882 if(!iter->second.getLabel().empty())
01883 {
01884 labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
01885 }
01886 }
01887
01888 std::map<int, Transform> poses = stat.poses();
01889 UDEBUG("time= %d ms", time.restart());
01890
01891 if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
01892 {
01893 _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
01894
01895 std::map<int, Signature>::const_iterator iter = stat.getSignatures().find(poses.rbegin()->first);
01896 if(iter != stat.getSignatures().end())
01897 {
01898 if(iter->second.sensorData().cameraModels().size() && !iter->second.sensorData().cameraModels()[0].localTransform().isNull())
01899 {
01900 _cloudViewer->updateCameraFrustums(poses.rbegin()->second, iter->second.sensorData().cameraModels());
01901 }
01902 else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull())
01903 {
01904 _cloudViewer->updateCameraFrustum(poses.rbegin()->second, iter->second.sensorData().stereoCameraModel());
01905 }
01906 }
01907
01908 if(_ui->graphicsView_graphView->isVisible())
01909 {
01910 _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
01911 }
01912 }
01913
01914 if(_cachedSignatures.contains(-1))
01915 {
01916 if(poses.find(stat.refImageId())!=poses.end())
01917 {
01918 poses.insert(std::make_pair(-1, poses.at(stat.refImageId())));
01919 poses.erase(stat.refImageId());
01920 }
01921 if(groundTruth.find(stat.refImageId())!=groundTruth.end())
01922 {
01923 groundTruth.insert(std::make_pair(-1, groundTruth.at(stat.refImageId())));
01924 groundTruth.erase(stat.refImageId());
01925 }
01926 }
01927
01928 std::map<std::string, float> updateCloudSats;
01929 updateMapCloud(
01930 poses,
01931 stat.constraints(),
01932 mapIds,
01933 labels,
01934 groundTruth,
01935 false,
01936 &updateCloudSats);
01937
01938 _odometryReceived = false;
01939
01940 UDEBUG("time= %d ms", time.restart());
01941
01942 for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
01943 {
01944 _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
01945 }
01946 }
01947 _odometryCorrection = stat.mapCorrection();
01948
01949 if( _ui->graphicsView_graphView->isVisible())
01950 {
01951
01952 if(_preferencesDialog->isPosteriorGraphView() &&
01953 stat.posterior().size())
01954 {
01955 _ui->graphicsView_graphView->updatePosterior(stat.posterior());
01956 }
01957 else if(_preferencesDialog->isRGBDMode())
01958 {
01959 if(_preferencesDialog->isWordsCountGraphView() &&
01960 _cachedWordsCount.size())
01961 {
01962 _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
01963 }
01964 else if(_preferencesDialog->isLocalizationsCountGraphView() &&
01965 _cachedLocalizationsCount.size())
01966 {
01967 _ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
01968 }
01969 }
01970
01971 _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
01972 if(stat.localPath().size() == 0)
01973 {
01974
01975 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
01976 }
01977
01978 if(stat.currentGoalId() > 0)
01979 {
01980 _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
01981 }
01982 }
01983 UDEBUG("");
01984
01985 _cachedSignatures.remove(-1);
01986
01987
01988 if(_cachedSignatures.contains(stat.refImageId()))
01989 {
01990 Signature & s = *_cachedSignatures.find(stat.refImageId());
01991 _cachedMemoryUsage -= s.sensorData().getMemoryUsed();
01992 s.sensorData().setImageRaw(cv::Mat());
01993 s.sensorData().setDepthOrRightRaw(cv::Mat());
01994 s.sensorData().setUserDataRaw(cv::Mat());
01995 s.sensorData().setLaserScanRaw(
01996 LaserScan(
01997 cv::Mat(),
01998 signature.sensorData().laserScanRaw().maxPoints(),
01999 signature.sensorData().laserScanRaw().maxRange(),
02000 signature.sensorData().laserScanRaw().format(),
02001 signature.sensorData().laserScanRaw().localTransform()));
02002 s.sensorData().clearOccupancyGridRaw();
02003 _cachedMemoryUsage += s.sensorData().getMemoryUsed();
02004 }
02005
02006 UDEBUG("");
02007 }
02008 else if(!stat.extended() && stat.loopClosureId()>0)
02009 {
02010 _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
02011 _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
02012 }
02013 else
02014 {
02015 _ui->label_matchId->clear();
02016 }
02017 float elapsedTime = static_cast<float>(totalTime.elapsed());
02018 UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
02019 _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
02020 if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
02021 {
02022 this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
02023 }
02024
02025 if(!_preferencesDialog->isImagesKept())
02026 {
02027 _cachedSignatures.clear();
02028 _cachedMemoryUsage = 0;
02029 _cachedWordsCount.clear();
02030 }
02031 _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
02032 _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
02033 #ifdef RTABMAP_OCTOMAP
02034 _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
02035 #endif
02036 if(_state != kMonitoring && _state != kDetecting)
02037 {
02038 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
02039 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
02040 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
02041 }
02042
02043 _processingStatistics = false;
02044
02045 Q_EMIT(statsProcessed());
02046 }
02047
02048 void MainWindow::updateMapCloud(
02049 const std::map<int, Transform> & posesIn,
02050 const std::multimap<int, Link> & constraints,
02051 const std::map<int, int> & mapIdsIn,
02052 const std::map<int, std::string> & labels,
02053 const std::map<int, Transform> & groundTruths,
02054 bool verboseProgress,
02055 std::map<std::string, float> * stats)
02056 {
02057 UTimer timer;
02058 UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
02059 (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
02060 if(posesIn.size())
02061 {
02062 _currentPosesMap = posesIn;
02063 _currentPosesMap.erase(-1);
02064 _currentLinksMap = constraints;
02065 _currentMapIds = mapIdsIn;
02066 _currentLabels = labels;
02067 _currentGTPosesMap = groundTruths;
02068 _currentGTPosesMap.erase(-1);
02069 if(_state != kMonitoring && _state != kDetecting)
02070 {
02071 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
02072 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
02073 }
02074 _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
02075 }
02076
02077
02078 std::map<int, Transform> poses;
02079 std::map<int, int> mapIds;
02080 if(_preferencesDialog->isCloudFiltering() && posesIn.size())
02081 {
02082 float radius = _preferencesDialog->getCloudFilteringRadius();
02083 float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0;
02084 bool hasNeg = posesIn.find(-1) != posesIn.end();
02085 if(hasNeg)
02086 {
02087 std::map<int, Transform> posesInTmp = posesIn;
02088 posesInTmp.erase(-1);
02089 poses = rtabmap::graph::radiusPosesFiltering(posesInTmp, radius, angle);
02090 }
02091 else
02092 {
02093 poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
02094 }
02095 for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
02096 {
02097 std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
02098 if(jter!=mapIdsIn.end())
02099 {
02100 mapIds.insert(*jter);
02101 }
02102 else
02103 {
02104 UERROR("map id of node %d not found!", iter->first);
02105 }
02106 }
02107
02108 if(hasNeg)
02109 {
02110 poses.insert(*posesIn.find(-1));
02111 }
02112
02113 if(verboseProgress)
02114 {
02115 _progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(posesIn.size()));
02116 QApplication::processEvents();
02117 }
02118 }
02119 else
02120 {
02121 poses = posesIn;
02122 mapIds = mapIdsIn;
02123 }
02124
02125 std::map<int, bool> posesMask;
02126 for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
02127 {
02128 posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
02129 }
02130 _ui->widget_mapVisibility->setMap(posesIn, posesMask);
02131
02132 if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
02133 {
02134 for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02135 {
02136 std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
02137 if(gtIter!=groundTruths.end())
02138 {
02139 iter->second = gtIter->second;
02140 }
02141 else
02142 {
02143 UWARN("Not found ground truth pose for node %d", iter->first);
02144 }
02145 }
02146 }
02147 else if(_currentGTPosesMap.size() == 0)
02148 {
02149 _ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
02150 }
02151
02152 int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
02153 if(maxNodes > 0 && poses.size()>1)
02154 {
02155 std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
02156 std::map<int, Transform> nearestPoses;
02157 nearestPoses.insert(*poses.rbegin());
02158 for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
02159 {
02160 std::map<int, Transform>::iterator pter = poses.find(*iter);
02161 if(pter != poses.end())
02162 {
02163 nearestPoses.insert(*pter);
02164 }
02165 }
02166
02167 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02168 {
02169 if(iter->first > 0)
02170 {
02171 break;
02172 }
02173 nearestPoses.insert(*iter);
02174 }
02175 poses=nearestPoses;
02176 }
02177
02178
02179 UDEBUG("Update map with %d locations", poses.size());
02180 QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
02181 int i=1;
02182 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02183 {
02184 if(!iter->second.isNull())
02185 {
02186 std::string cloudName = uFormat("cloud%d", iter->first);
02187
02188 if(iter->first < 0)
02189 {
02190 viewerClouds.remove(cloudName);
02191 _cloudViewer->removeCloud(cloudName);
02192 }
02193
02194
02195 bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
02196 if(update3dCloud)
02197 {
02198
02199 if(viewerClouds.contains(cloudName))
02200 {
02201
02202 Transform tCloud;
02203 _cloudViewer->getPose(cloudName, tCloud);
02204 if(tCloud.isNull() || iter->second != tCloud)
02205 {
02206 if(!_cloudViewer->updateCloudPose(cloudName, iter->second))
02207 {
02208 UERROR("Updating pose cloud %d failed!", iter->first);
02209 }
02210 }
02211 _cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
02212 _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
02213 _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
02214 _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
02215 }
02216 else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
02217 _cachedClouds.find(iter->first) == _cachedClouds.end() &&
02218 _cachedSignatures.contains(iter->first))
02219 {
02220 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
02221 if(_cloudViewer->getAddedClouds().contains(cloudName))
02222 {
02223 _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
02224 }
02225 }
02226 }
02227 else if(viewerClouds.contains(cloudName))
02228 {
02229 _cloudViewer->setCloudVisibility(cloudName.c_str(), false);
02230 }
02231
02232
02233 std::string scanName = uFormat("scan%d", iter->first);
02234 if(iter->first < 0)
02235 {
02236 viewerClouds.remove(scanName);
02237 _cloudViewer->removeCloud(scanName);
02238 }
02239 if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
02240 {
02241 if(viewerClouds.contains(scanName))
02242 {
02243
02244 Transform tScan;
02245 _cloudViewer->getPose(scanName, tScan);
02246 if(tScan.isNull() || iter->second != tScan)
02247 {
02248 if(!_cloudViewer->updateCloudPose(scanName, iter->second))
02249 {
02250 UERROR("Updating pose scan %d failed!", iter->first);
02251 }
02252 }
02253 _cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
02254 _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
02255 _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
02256 _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
02257 }
02258 else if(_cachedSignatures.contains(iter->first))
02259 {
02260 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
02261 if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
02262 {
02263 this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
02264 }
02265 }
02266 }
02267 else if(viewerClouds.contains(scanName))
02268 {
02269 _cloudViewer->setCloudVisibility(scanName.c_str(), false);
02270 }
02271
02272
02273 bool updateGridMap =
02274 ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
02275 (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
02276 _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
02277 bool updateOctomap = false;
02278 #ifdef RTABMAP_OCTOMAP
02279 updateOctomap =
02280 _cloudViewer->isVisible() &&
02281 _preferencesDialog->isOctomapUpdated() &&
02282 _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
02283 #endif
02284 if(updateGridMap || updateOctomap)
02285 {
02286 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
02287 if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
02288 {
02289 cv::Mat ground;
02290 cv::Mat obstacles;
02291 cv::Mat empty;
02292
02293 jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
02294
02295 _occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
02296
02297 #ifdef RTABMAP_OCTOMAP
02298 if(updateOctomap)
02299 {
02300 if((ground.empty() || ground.channels() > 2) &&
02301 (obstacles.empty() || obstacles.channels() > 2))
02302 {
02303 cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
02304 _octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
02305 }
02306 else if(!ground.empty() || !obstacles.empty())
02307 {
02308 UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
02309 }
02310 }
02311 #endif
02312 }
02313 }
02314
02315
02316 std::string featuresName = uFormat("features%d", iter->first);
02317 if(iter->first < 0)
02318 {
02319 viewerClouds.remove(featuresName);
02320 _cloudViewer->removeCloud(featuresName);
02321 }
02322 if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
02323 {
02324 if(viewerClouds.contains(featuresName))
02325 {
02326
02327 Transform tFeatures;
02328 _cloudViewer->getPose(featuresName, tFeatures);
02329 if(tFeatures.isNull() || iter->second != tFeatures)
02330 {
02331 if(!_cloudViewer->updateCloudPose(featuresName, iter->second))
02332 {
02333 UERROR("Updating pose features %d failed!", iter->first);
02334 }
02335 }
02336 _cloudViewer->setCloudVisibility(featuresName, _preferencesDialog->isFeaturesShown(0));
02337 _cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
02338 }
02339 else if(_cachedSignatures.contains(iter->first))
02340 {
02341 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
02342 if(!jter->getWords3().empty())
02343 {
02344 this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
02345 }
02346 }
02347 }
02348 else if(viewerClouds.contains(featuresName))
02349 {
02350 _cloudViewer->setCloudVisibility(featuresName.c_str(), false);
02351 }
02352
02353 if(verboseProgress)
02354 {
02355 _progressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
02356 _progressDialog->incrementStep();
02357 if(poses.size() < 200 || i % 100 == 0)
02358 {
02359 QApplication::processEvents();
02360 if(_progressCanceled)
02361 {
02362 break;
02363 }
02364 }
02365 }
02366 }
02367
02368 ++i;
02369 }
02370
02371
02372 for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
02373 {
02374 std::list<std::string> splitted = uSplitNumChar(iter.key());
02375 int id = 0;
02376 if(splitted.size() == 2)
02377 {
02378 id = std::atoi(splitted.back().c_str());
02379 if(splitted.front().at(splitted.front().size()-1) == '-')
02380 {
02381 id*=-1;
02382 }
02383 }
02384
02385 if(id != 0 && poses.find(id) == poses.end())
02386 {
02387 if(_cloudViewer->getCloudVisibility(iter.key()))
02388 {
02389 UDEBUG("Hide %s", iter.key().c_str());
02390 _cloudViewer->setCloudVisibility(iter.key(), false);
02391 }
02392 }
02393 }
02394
02395 UDEBUG("");
02396 if(stats)
02397 {
02398 stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f));
02399 }
02400
02401
02402 _cloudViewer->removeAllGraphs();
02403 _cloudViewer->removeCloud("graph_nodes");
02404 if(!_preferencesDialog->isFrustumsShown(0))
02405 {
02406 QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
02407 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
02408 {
02409 std::list<std::string> splitted = uSplitNumChar(iter.key());
02410 if(splitted.size() == 2)
02411 {
02412 if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0))
02413 {
02414 _cloudViewer->removeFrustum(iter.key());
02415 }
02416 }
02417 }
02418 }
02419
02420 Transform mapToGt = Transform::getIdentity();
02421 if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
02422 {
02423 mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
02424 }
02425
02426 if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
02427 {
02428 UTimer timerGraph;
02429
02430 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
02431 for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
02432 {
02433 int mapId = uValue(_currentMapIds, iter->first, -1);
02434
02435 if(_preferencesDialog->isGraphsShown())
02436 {
02437
02438 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
02439 if(kter == graphs.end())
02440 {
02441 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
02442 }
02443 pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
02444 kter->second->push_back(pt);
02445 }
02446
02447
02448 if(_preferencesDialog->isFrustumsShown(0))
02449 {
02450 std::string frustumId = uFormat("f_%d", iter->first);
02451 if(_cloudViewer->getAddedFrustums().contains(frustumId))
02452 {
02453 _cloudViewer->updateFrustumPose(frustumId, iter->second);
02454 }
02455 else if(_cachedSignatures.contains(iter->first))
02456 {
02457 const Signature & s = _cachedSignatures.value(iter->first);
02458
02459 if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
02460 {
02461 Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
02462 if(!t.isNull())
02463 {
02464 QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
02465 _cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
02466
02467 if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
02468 {
02469 std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
02470 color = Qt::gray;
02471 _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color);
02472 }
02473 }
02474 }
02475 }
02476 }
02477 }
02478
02479
02480 for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
02481 {
02482 int mapId = -100;
02483
02484 if(_preferencesDialog->isGraphsShown())
02485 {
02486
02487 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
02488 if(kter == graphs.end())
02489 {
02490 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
02491 }
02492 Transform t = mapToGt*iter->second;
02493 pcl::PointXYZ pt(t.x(), t.y(), t.z());
02494 kter->second->push_back(pt);
02495 }
02496 }
02497
02498
02499 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
02500 {
02501 QColor color = Qt::gray;
02502 if(iter->first >= 0)
02503 {
02504 color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
02505 }
02506 _cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
02507 }
02508
02509 if(_preferencesDialog->isFrustumsShown(0))
02510 {
02511 QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
02512 UDEBUG("remove not used frustums");
02513 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
02514 {
02515 std::list<std::string> splitted = uSplitNumChar(iter.key());
02516 if(splitted.size() == 2)
02517 {
02518 int id = std::atoi(splitted.back().c_str());
02519 if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
02520 _currentPosesMap.find(id) == _currentPosesMap.end())
02521 {
02522 _cloudViewer->removeFrustum(iter.key());
02523 }
02524 }
02525 }
02526 }
02527
02528 UDEBUG("timerGraph=%fs", timerGraph.ticks());
02529 }
02530
02531 UDEBUG("labels.size()=%d", (int)labels.size());
02532
02533
02534 _cloudViewer->removeAllTexts();
02535 if(_preferencesDialog->isLabelsShown() && labels.size())
02536 {
02537 for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
02538 {
02539 if(posesIn.find(iter->first)!=posesIn.end())
02540 {
02541 int mapId = uValue(mapIdsIn, iter->first, -1);
02542 QColor color = Qt::gray;
02543 if(mapId >= 0)
02544 {
02545 color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
02546 }
02547 _cloudViewer->addOrUpdateText(
02548 std::string("label_") + uNumber2Str(iter->first),
02549 iter->second,
02550 _currentPosesMap.at(iter->first),
02551 0.1,
02552 color);
02553 }
02554 }
02555 }
02556
02557 UDEBUG("");
02558 if(stats)
02559 {
02560 stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f));
02561 }
02562
02563 #ifdef RTABMAP_OCTOMAP
02564 _cloudViewer->removeOctomap();
02565 _cloudViewer->removeCloud("octomap_cloud");
02566 if(_preferencesDialog->isOctomapUpdated())
02567 {
02568 UDEBUG("");
02569 UTimer time;
02570 _octomap->update(poses);
02571 UINFO("Octomap update time = %fs", time.ticks());
02572 }
02573 if(stats)
02574 {
02575 stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f));
02576 }
02577 if(_preferencesDialog->isOctomapShown())
02578 {
02579 UDEBUG("");
02580 UTimer time;
02581 if(_preferencesDialog->getOctomapRenderingType() > 0)
02582 {
02583 _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
02584 }
02585 else
02586 {
02587 pcl::IndicesPtr obstacles(new std::vector<int>);
02588 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = _octomap->createCloud(_preferencesDialog->getOctomapTreeDepth(), obstacles.get());
02589 if(obstacles->size())
02590 {
02591 _cloudViewer->addCloud("octomap_cloud", cloud);
02592 _cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize());
02593 }
02594 }
02595 UINFO("Octomap show 3d map time = %fs", time.ticks());
02596 }
02597 UDEBUG("");
02598 if(stats)
02599 {
02600 stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f));
02601 }
02602 #endif
02603
02604
02605 if(_ui->graphicsView_graphView->isVisible())
02606 {
02607 _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
02608 if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
02609 {
02610 std::map<int, Transform> gtPoses = _currentGTPosesMap;
02611 for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
02612 {
02613 iter->second = mapToGt * iter->second;
02614 }
02615 _ui->graphicsView_graphView->updateGTGraph(gtPoses);
02616 }
02617 else
02618 {
02619 _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
02620 }
02621 }
02622 cv::Mat map8U;
02623 if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
02624 {
02625 float xMin, yMin;
02626 float resolution = _occupancyGrid->getCellSize();
02627 cv::Mat map8S;
02628 #ifdef RTABMAP_OCTOMAP
02629 if(_preferencesDialog->isOctomap2dGrid())
02630 {
02631 map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
02632
02633 }
02634 else
02635 #endif
02636 {
02637 if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0)
02638 {
02639 _occupancyGrid->update(poses);
02640 }
02641 if(stats)
02642 {
02643 stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
02644 }
02645 map8S = _occupancyGrid->getMap(xMin, yMin);
02646 }
02647 if(!map8S.empty())
02648 {
02649
02650 map8U = util3d::convertMap2Image8U(map8S);
02651
02652 if(_preferencesDialog->getGridMapShown())
02653 {
02654 float opacity = _preferencesDialog->getGridMapOpacity();
02655 _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
02656 }
02657 if(_ui->graphicsView_graphView->isVisible())
02658 {
02659 _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
02660 }
02661 }
02662 }
02663 _ui->graphicsView_graphView->update();
02664
02665 UDEBUG("");
02666 if(stats)
02667 {
02668 stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f));
02669 }
02670
02671 if(!_preferencesDialog->getGridMapShown())
02672 {
02673 UDEBUG("");
02674 _cloudViewer->removeOccupancyGridMap();
02675 }
02676
02677 if(viewerClouds.contains("cloudOdom"))
02678 {
02679 if(!_preferencesDialog->isCloudsShown(1))
02680 {
02681 UDEBUG("");
02682 _cloudViewer->setCloudVisibility("cloudOdom", false);
02683 }
02684 else
02685 {
02686 UDEBUG("");
02687 _cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
02688 _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
02689 _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
02690 _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
02691 }
02692 }
02693 if(viewerClouds.contains("scanOdom"))
02694 {
02695 if(!_preferencesDialog->isScansShown(1))
02696 {
02697 UDEBUG("");
02698 _cloudViewer->setCloudVisibility("scanOdom", false);
02699 }
02700 else
02701 {
02702 UDEBUG("");
02703 _cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
02704 _cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
02705 _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
02706 _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
02707 }
02708 }
02709 if(viewerClouds.contains("scanMapOdom"))
02710 {
02711 if(!_preferencesDialog->isScansShown(1))
02712 {
02713 UDEBUG("");
02714 _cloudViewer->setCloudVisibility("scanMapOdom", false);
02715 }
02716 else if(_cloudViewer->getBackgroundColor() != Qt::darkRed)
02717 {
02718 UDEBUG("");
02719 _cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
02720 _cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
02721 _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
02722 _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
02723 }
02724 }
02725 if(viewerClouds.contains("featuresOdom"))
02726 {
02727 if(!_preferencesDialog->isFeaturesShown(1))
02728 {
02729 UDEBUG("");
02730 _cloudViewer->setCloudVisibility("featuresOdom", false);
02731 }
02732 else if(_cloudViewer->getBackgroundColor() != Qt::darkRed)
02733 {
02734 UDEBUG("");
02735 _cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
02736 _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
02737 }
02738 }
02739
02740
02741 if(_state != kMonitoring && _state != kDetecting)
02742 {
02743 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
02744 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
02745 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
02746 #ifdef RTABMAP_OCTOMAP
02747 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
02748 #else
02749 _ui->actionExport_octomap->setEnabled(false);
02750 #endif
02751 }
02752
02753 UDEBUG("");
02754 _cloudViewer->update();
02755 UDEBUG("");
02756 }
02757
02758 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
02759 {
02760 UASSERT(!pose.isNull());
02761 std::string cloudName = uFormat("cloud%d", nodeId);
02762 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
02763 if(_cloudViewer->getAddedClouds().contains(cloudName))
02764 {
02765 UERROR("Cloud %d already added to map.", nodeId);
02766 return outputPair;
02767 }
02768
02769 QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
02770 if(iter == _cachedSignatures.end())
02771 {
02772 UERROR("Node %d is not in the cache.", nodeId);
02773 return outputPair;
02774 }
02775
02776 UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
02777
02778 if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
02779 (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
02780 {
02781 cv::Mat image, depth;
02782 SensorData data = iter->sensorData();
02783 data.uncompressData(&image, &depth, 0);
02784 ParametersMap allParameters = _preferencesDialog->getAllParameters();
02785 bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
02786 bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
02787 Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
02788 Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
02789 if(rectifyOnlyFeatures && !imagesAlreadyRectified)
02790 {
02791 if(data.cameraModels().size())
02792 {
02793 UTimer time;
02794
02795 UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
02796 int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
02797 cv::Mat rectifiedImages = data.imageRaw().clone();
02798 bool initRectMaps = _rectCameraModels.empty();
02799 if(initRectMaps)
02800 {
02801 _rectCameraModels.resize(data.cameraModels().size());
02802 }
02803 for(unsigned int i=0; i<data.cameraModels().size(); ++i)
02804 {
02805 if(data.cameraModels()[i].isValidForRectification())
02806 {
02807 if(initRectMaps)
02808 {
02809 _rectCameraModels[i] = data.cameraModels()[i];
02810 if(!_rectCameraModels[i].isRectificationMapInitialized())
02811 {
02812 UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
02813 _rectCameraModels[i].initRectificationMap();
02814 UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
02815 }
02816 }
02817 UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
02818 _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
02819 cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
02820 rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
02821 }
02822 else
02823 {
02824 UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
02825 i, data.id(),
02826 data.cameraModels()[i].imageWidth(),
02827 data.cameraModels()[i].imageHeight());
02828 }
02829 }
02830 UINFO("Time rectification: %fs", time.ticks());
02831 data.setImageRaw(rectifiedImages);
02832 image = rectifiedImages;
02833 }
02834 }
02835
02836 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
02837 pcl::IndicesPtr indices(new std::vector<int>);
02838 UASSERT_MSG(nodeId == -1 || nodeId == data.id(), uFormat("nodeId=%d data.id()=%d", nodeId, data.id()).c_str());
02839
02840
02841 cloud = util3d::cloudRGBFromSensorData(data,
02842 _preferencesDialog->getCloudDecimation(0),
02843 _preferencesDialog->getCloudMaxDepth(0),
02844 _preferencesDialog->getCloudMinDepth(0),
02845 indices.get(),
02846 allParameters,
02847 _preferencesDialog->getCloudRoiRatios(0));
02848
02849
02850 Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
02851 if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
02852 {
02853 viewPoint[0] = data.cameraModels()[0].localTransform().x();
02854 viewPoint[1] = data.cameraModels()[0].localTransform().y();
02855 viewPoint[2] = data.cameraModels()[0].localTransform().z();
02856 }
02857 else if(!data.stereoCameraModel().localTransform().isNull())
02858 {
02859 viewPoint[0] = data.stereoCameraModel().localTransform().x();
02860 viewPoint[1] = data.stereoCameraModel().localTransform().y();
02861 viewPoint[2] = data.stereoCameraModel().localTransform().z();
02862 }
02863
02864
02865 if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
02866 {
02867 cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
02868
02869 indices->resize(cloud->size());
02870 for(unsigned int i=0; i<cloud->size(); ++i)
02871 {
02872 indices->at(i) = i;
02873 }
02874 }
02875
02876
02877 if(indices->size() &&
02878 (_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
02879 _preferencesDialog->getCeilingFilteringHeight() != 0.0))
02880 {
02881
02882 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
02883 indices = rtabmap::util3d::passThrough(
02884 cloudTransformed,
02885 indices,
02886 "z",
02887 _preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
02888 _preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
02889 }
02890
02891
02892 if(indices->size() &&
02893 _preferencesDialog->getNoiseRadius() > 0.0 &&
02894 _preferencesDialog->getNoiseMinNeighbors() > 0)
02895 {
02896 indices = rtabmap::util3d::radiusFiltering(
02897 cloud,
02898 indices,
02899 _preferencesDialog->getNoiseRadius(),
02900 _preferencesDialog->getNoiseMinNeighbors());
02901 }
02902
02903 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02904 if(_preferencesDialog->isSubtractFiltering() &&
02905 _preferencesDialog->getSubtractFilteringRadius() > 0.0 &&
02906 nodeId > 0)
02907 {
02908 pcl::IndicesPtr beforeFiltering = indices;
02909 if( cloud->size() &&
02910 _previousCloud.first>0 &&
02911 _previousCloud.second.first.first.get() != 0 &&
02912 _previousCloud.second.second.get() != 0 &&
02913 _previousCloud.second.second->size() &&
02914 _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
02915 {
02916 UTimer time;
02917
02918 rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
02919
02920
02921
02922
02923
02924 if(_preferencesDialog->isSubtractFiltering())
02925 {
02926 if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
02927 {
02928
02929 if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
02930 {
02931 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
02932 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
02933 }
02934 else
02935 {
02936 UWARN("Cloud subtraction with angle filtering is activated but "
02937 "cloud normal K search is 0. Subtraction is done with angle.");
02938 }
02939 }
02940
02941 if(cloudWithNormals->size() &&
02942 _previousCloud.second.first.second.get() &&
02943 _previousCloud.second.first.second->size())
02944 {
02945 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t);
02946 indices = rtabmap::util3d::subtractFiltering(
02947 cloudWithNormals,
02948 indices,
02949 previousCloud,
02950 _previousCloud.second.second,
02951 _preferencesDialog->getSubtractFilteringRadius(),
02952 _preferencesDialog->getSubtractFilteringAngle(),
02953 _preferencesDialog->getSubtractFilteringMinPts());
02954 }
02955 else
02956 {
02957 pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t);
02958 indices = rtabmap::util3d::subtractFiltering(
02959 cloud,
02960 indices,
02961 previousCloud,
02962 _previousCloud.second.second,
02963 _preferencesDialog->getSubtractFilteringRadius(),
02964 _preferencesDialog->getSubtractFilteringMinPts());
02965 }
02966
02967
02968 UINFO("Time subtract filtering %d from %d -> %d (%fs)",
02969 (int)_previousCloud.second.second->size(),
02970 (int)beforeFiltering->size(),
02971 (int)indices->size(),
02972 time.ticks());
02973 }
02974 }
02975
02976 _previousCloud.first = nodeId;
02977 _previousCloud.second.first.first = cloud;
02978 _previousCloud.second.first.second = cloudWithNormals;
02979 _previousCloud.second.second = beforeFiltering;
02980 }
02981
02982 if(indices->size())
02983 {
02984 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
02985 bool added = false;
02986 if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
02987 {
02988
02989
02990 output = util3d::extractIndices(cloud, indices, false, true);
02991 std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
02992 output,
02993 _preferencesDialog->getCloudMeshingAngle(),
02994 _preferencesDialog->isCloudMeshingQuad(),
02995 _preferencesDialog->getCloudMeshingTriangleSize(),
02996 viewPoint);
02997 if(polygons.size())
02998 {
02999
03000 pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
03001 std::vector<pcl::Vertices> outputPolygons;
03002 std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
03003
03004 if(_preferencesDialog->isCloudMeshingTexture() && !image.empty())
03005 {
03006 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
03007 pcl::toPCLPointCloud2(*outputFiltered, textureMesh->cloud);
03008 textureMesh->tex_polygons.push_back(outputPolygons);
03009 int w = cloud->width;
03010 int h = cloud->height;
03011 UASSERT(w > 1 && h > 1);
03012 textureMesh->tex_coordinates.resize(1);
03013 int nPoints = (int)outputFiltered->size();
03014 textureMesh->tex_coordinates[0].resize(nPoints);
03015 for(int i=0; i<nPoints; ++i)
03016 {
03017
03018 UASSERT(i < (int)denseToOrganizedIndices.size());
03019 int originalVertex = denseToOrganizedIndices[i];
03020 textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
03021 float(originalVertex % w) / float(w),
03022 float(h - originalVertex / w) / float(h));
03023 }
03024
03025 pcl::TexMaterial mesh_material;
03026 mesh_material.tex_d = 1.0f;
03027 mesh_material.tex_Ns = 75.0f;
03028 mesh_material.tex_illum = 1;
03029
03030 std::stringstream tex_name;
03031 tex_name << "material_" << nodeId;
03032 tex_name >> mesh_material.tex_name;
03033
03034 mesh_material.tex_file = "";
03035
03036 textureMesh->tex_materials.push_back(mesh_material);
03037
03038 if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, image, pose))
03039 {
03040 UERROR("Adding texture mesh %d to viewer failed!", nodeId);
03041 }
03042 else
03043 {
03044 added = true;
03045 }
03046 }
03047 else if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
03048 {
03049 UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
03050 }
03051 else
03052 {
03053 added = true;
03054 }
03055 }
03056 }
03057 else
03058 {
03059 if(_preferencesDialog->isCloudMeshing())
03060 {
03061 UWARN("Online meshing is activated but the generated cloud is "
03062 "dense (voxel filtering is used or multiple cameras are used). Disable "
03063 "online meshing in Preferences->3D Rendering to hide this warning.");
03064 }
03065
03066 if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
03067 {
03068 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
03069 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
03070 }
03071
03072 QColor color = Qt::gray;
03073 if(mapId >= 0)
03074 {
03075 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
03076 }
03077
03078 output = util3d::extractIndices(cloud, indices, false, true);
03079
03080 if(cloudWithNormals->size())
03081 {
03082 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
03083 outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false);
03084
03085 if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color))
03086 {
03087 UERROR("Adding cloud %d to viewer failed!", nodeId);
03088 }
03089 else
03090 {
03091 added = true;
03092 }
03093 }
03094 else
03095 {
03096 if(!_cloudViewer->addCloud(cloudName, output, pose, color))
03097 {
03098 UERROR("Adding cloud %d to viewer failed!", nodeId);
03099 }
03100 else
03101 {
03102 added = true;
03103 }
03104 }
03105 }
03106 if(added)
03107 {
03108 outputPair.first = output;
03109 outputPair.second = indices;
03110 if(_preferencesDialog->isCloudsKept() && nodeId > 0)
03111 {
03112 _cachedClouds.insert(std::make_pair(nodeId, outputPair));
03113 _createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
03114 }
03115 _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
03116 _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
03117 _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
03118 }
03119 else if(nodeId>0)
03120 {
03121 _cachedEmptyClouds.insert(nodeId);
03122 }
03123 }
03124 else if(nodeId>0)
03125 {
03126 _cachedEmptyClouds.insert(nodeId);
03127 }
03128 }
03129
03130 return outputPair;
03131 }
03132
03133 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
03134 {
03135 std::string scanName = uFormat("scan%d", nodeId);
03136 if(_cloudViewer->getAddedClouds().contains(scanName))
03137 {
03138 UERROR("Scan %d already added to map.", nodeId);
03139 return;
03140 }
03141
03142 QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
03143 if(iter == _cachedSignatures.end())
03144 {
03145 UERROR("Node %d is not in the cache.", nodeId);
03146 return;
03147 }
03148
03149 if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
03150 {
03151 LaserScan scan;
03152 iter->sensorData().uncompressData(0, 0, &scan);
03153
03154 if(_preferencesDialog->getDownsamplingStepScan(0) > 1 ||
03155 _preferencesDialog->getScanMaxRange(0) > 0.0f ||
03156 _preferencesDialog->getScanMinRange(0) > 0.0f)
03157 {
03158 scan = util3d::commonFiltering(scan,
03159 _preferencesDialog->getDownsamplingStepScan(0),
03160 _preferencesDialog->getScanMinRange(0),
03161 _preferencesDialog->getScanMaxRange(0));
03162 }
03163
03164 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
03165 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
03166 pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
03167 pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
03168 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
03169 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
03170 if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
03171 {
03172 cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
03173 }
03174 else if(scan.hasNormals() && scan.hasIntensity() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
03175 {
03176 cloudIWithNormals = util3d::laserScanToPointCloudINormal(scan, scan.localTransform());
03177 }
03178 else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
03179 {
03180 cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
03181 }
03182 else if(scan.hasRGB())
03183 {
03184 cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
03185 }
03186 else if(scan.hasIntensity())
03187 {
03188 cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
03189 }
03190 else
03191 {
03192 cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
03193 }
03194
03195 if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
03196 {
03197 if(cloud.get())
03198 {
03199 cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
03200 }
03201 if(cloudRGB.get())
03202 {
03203 cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
03204 }
03205 if(cloudI.get())
03206 {
03207 cloudI = util3d::voxelize(cloudI, _preferencesDialog->getCloudVoxelSizeScan(0));
03208 }
03209 }
03210
03211
03212 if((!scan.is2d()) &&
03213 (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
03214 _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
03215 {
03216 if(cloudRGBWithNormals.get())
03217 {
03218
03219 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
03220 cloudTransformed = rtabmap::util3d::passThrough(
03221 cloudTransformed,
03222 "z",
03223 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03224 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03225
03226
03227 cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03228 }
03229 if(cloudIWithNormals.get())
03230 {
03231
03232 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudIWithNormals, pose);
03233 cloudTransformed = rtabmap::util3d::passThrough(
03234 cloudTransformed,
03235 "z",
03236 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03237 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03238
03239
03240 cloudIWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03241 }
03242 if(cloudWithNormals.get())
03243 {
03244
03245 pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
03246 cloudTransformed = rtabmap::util3d::passThrough(
03247 cloudTransformed,
03248 "z",
03249 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03250 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03251
03252
03253 cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03254 }
03255 if(cloudRGB.get())
03256 {
03257
03258 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
03259 cloudTransformed = rtabmap::util3d::passThrough(
03260 cloudTransformed,
03261 "z",
03262 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03263 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03264
03265
03266 cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03267 }
03268 if(cloudI.get())
03269 {
03270
03271 pcl::PointCloud<pcl::PointXYZI>::Ptr cloudTransformed = util3d::transformPointCloud(cloudI, pose);
03272 cloudTransformed = rtabmap::util3d::passThrough(
03273 cloudTransformed,
03274 "z",
03275 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03276 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03277
03278
03279 cloudI = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03280 }
03281 if(cloud.get())
03282 {
03283
03284 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
03285 cloudTransformed = rtabmap::util3d::passThrough(
03286 cloudTransformed,
03287 "z",
03288 _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
03289 _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
03290
03291
03292 cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
03293 }
03294 }
03295
03296 if( (cloud.get() || cloudRGB.get() || cloudI.get()) &&
03297 (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
03298 {
03299 Eigen::Vector3f scanViewpoint(
03300 scan.localTransform().x(),
03301 scan.localTransform().y(),
03302 scan.localTransform().z());
03303
03304 pcl::PointCloud<pcl::Normal>::Ptr normals;
03305 if(cloud.get() && cloud->size())
03306 {
03307 if(scan.is2d())
03308 {
03309 normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
03310 }
03311 else
03312 {
03313 normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
03314 }
03315 cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
03316 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
03317 cloud.reset();
03318 }
03319 else if(cloudRGB.get() && cloudRGB->size())
03320 {
03321
03322 normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
03323 cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
03324 pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
03325 cloudRGB.reset();
03326 }
03327 else if(cloudI.get())
03328 {
03329 if(scan.is2d())
03330 {
03331 normals = util3d::computeFastOrganizedNormals2D(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
03332 }
03333 else
03334 {
03335 normals = util3d::computeNormals(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
03336 }
03337 cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
03338 pcl::concatenateFields(*cloud, *normals, *cloudIWithNormals);
03339 cloudI.reset();
03340 }
03341 }
03342
03343 QColor color = Qt::gray;
03344 if(mapId >= 0)
03345 {
03346 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
03347 }
03348 bool added = false;
03349 if(cloudRGBWithNormals.get())
03350 {
03351 added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
03352 if(added && nodeId > 0)
03353 {
03354 scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
03355 }
03356 }
03357 else if(cloudIWithNormals.get())
03358 {
03359 added = _cloudViewer->addCloud(scanName, cloudIWithNormals, pose, color);
03360 if(added && nodeId > 0)
03361 {
03362 if(scan.is2d())
03363 {
03364 scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
03365 }
03366 else
03367 {
03368 scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
03369 }
03370 }
03371 }
03372 else if(cloudWithNormals.get())
03373 {
03374 added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
03375 if(added && nodeId > 0)
03376 {
03377 if(scan.is2d())
03378 {
03379 scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
03380 }
03381 else
03382 {
03383 scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
03384 }
03385 }
03386 }
03387 else if(cloudRGB.get())
03388 {
03389 added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
03390 if(added && nodeId > 0)
03391 {
03392 scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
03393 }
03394 }
03395 else if(cloudI.get())
03396 {
03397 added = _cloudViewer->addCloud(scanName, cloudI, pose, color);
03398 if(added && nodeId > 0)
03399 {
03400 if(scan.is2d())
03401 {
03402 scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYI, scan.localTransform());
03403 }
03404 else
03405 {
03406 scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
03407 }
03408 }
03409 }
03410 else
03411 {
03412 UASSERT(cloud.get());
03413 added = _cloudViewer->addCloud(scanName, cloud, pose, color);
03414 if(added && nodeId > 0)
03415 {
03416 if(scan.is2d())
03417 {
03418 scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXY, scan.localTransform());
03419 }
03420 else
03421 {
03422 scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
03423 }
03424 }
03425 }
03426 if(!added)
03427 {
03428 UERROR("Adding cloud %d to viewer failed!", nodeId);
03429 }
03430 else
03431 {
03432 if(nodeId > 0)
03433 {
03434 _createdScans.insert(std::make_pair(nodeId, scan));
03435 }
03436
03437 _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(0));
03438 _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
03439 _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
03440 }
03441 }
03442 }
03443
03444 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
03445 {
03446 UDEBUG("");
03447 UASSERT(!pose.isNull());
03448 std::string cloudName = uFormat("features%d", nodeId);
03449 if(_cloudViewer->getAddedClouds().contains(cloudName))
03450 {
03451 UERROR("Features cloud %d already added to map.", nodeId);
03452 return;
03453 }
03454
03455 QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
03456 if(iter == _cachedSignatures.end())
03457 {
03458 UERROR("Node %d is not in the cache.", nodeId);
03459 return;
03460 }
03461
03462 if(_createdFeatures.find(nodeId) != _createdFeatures.end())
03463 {
03464 UDEBUG("Features cloud %d already created.");
03465 return;
03466 }
03467
03468 if(iter->getWords3().size())
03469 {
03470 UINFO("Create cloud from 3D words");
03471 QColor color = Qt::gray;
03472 if(mapId >= 0)
03473 {
03474 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
03475 }
03476
03477 cv::Mat rgb;
03478 if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
03479 {
03480 SensorData data = iter->sensorData();
03481 data.uncompressData(&rgb, 0, 0);
03482 }
03483
03484 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
03485 cloud->resize(iter->getWords3().size());
03486 int oi=0;
03487 UASSERT(iter->getWords().size() == iter->getWords3().size());
03488 std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
03489 float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
03490 UDEBUG("rgb.channels()=%d");
03491 for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
03492 jter!=iter->getWords3().end(); ++jter, ++kter)
03493 {
03494 if(util3d::isFinite(jter->second) && (maxDepth == 0.0f || jter->second.z < maxDepth))
03495 {
03496 (*cloud)[oi].x = jter->second.x;
03497 (*cloud)[oi].y = jter->second.y;
03498 (*cloud)[oi].z = jter->second.z;
03499 int u = kter->second.pt.x+0.5;
03500 int v = kter->second.pt.y+0.5;
03501 if(!rgb.empty() &&
03502 uIsInBounds(u, 0, rgb.cols-1) &&
03503 uIsInBounds(v, 0, rgb.rows-1))
03504 {
03505 if(rgb.channels() == 1)
03506 {
03507 (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
03508 }
03509 else
03510 {
03511 cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
03512 (*cloud)[oi].b = bgr.val[0];
03513 (*cloud)[oi].g = bgr.val[1];
03514 (*cloud)[oi].r = bgr.val[2];
03515 }
03516 }
03517 else
03518 {
03519 (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
03520 }
03521 ++oi;
03522 }
03523 }
03524 cloud->resize(oi);
03525 if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
03526 {
03527 UERROR("Adding features cloud %d to viewer failed!", nodeId);
03528 }
03529 else if(nodeId > 0)
03530 {
03531 _createdFeatures.insert(std::make_pair(nodeId, cloud));
03532 }
03533 }
03534 else
03535 {
03536 return;
03537 }
03538
03539 _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
03540 UDEBUG("");
03541 }
03542
03543 Transform MainWindow::alignPosesToGroundTruth(
03544 const std::map<int, Transform> & poses,
03545 const std::map<int, Transform> & groundTruth)
03546 {
03547 Transform t = Transform::getIdentity();
03548 if(groundTruth.size() && poses.size())
03549 {
03550 float translational_rmse = 0.0f;
03551 float translational_mean = 0.0f;
03552 float translational_median = 0.0f;
03553 float translational_std = 0.0f;
03554 float translational_min = 0.0f;
03555 float translational_max = 0.0f;
03556 float rotational_rmse = 0.0f;
03557 float rotational_mean = 0.0f;
03558 float rotational_median = 0.0f;
03559 float rotational_std = 0.0f;
03560 float rotational_min = 0.0f;
03561 float rotational_max = 0.0f;
03562
03563 t = graph::calcRMSE(
03564 groundTruth,
03565 poses,
03566 translational_rmse,
03567 translational_mean,
03568 translational_median,
03569 translational_std,
03570 translational_min,
03571 translational_max,
03572 rotational_rmse,
03573 rotational_mean,
03574 rotational_median,
03575 rotational_std,
03576 rotational_min,
03577 rotational_max);
03578
03579
03580 UINFO("translational_rmse=%f", translational_rmse);
03581 UINFO("translational_mean=%f", translational_mean);
03582 UINFO("translational_median=%f", translational_median);
03583 UINFO("translational_std=%f", translational_std);
03584 UINFO("translational_min=%f", translational_min);
03585 UINFO("translational_max=%f", translational_max);
03586
03587 UINFO("rotational_rmse=%f", rotational_rmse);
03588 UINFO("rotational_mean=%f", rotational_mean);
03589 UINFO("rotational_median=%f", rotational_median);
03590 UINFO("rotational_std=%f", rotational_std);
03591 UINFO("rotational_min=%f", rotational_min);
03592 UINFO("rotational_max=%f", rotational_max);
03593 }
03594 return t;
03595 }
03596
03597 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
03598 {
03599 UINFO("Update visibility %d", nodeId);
03600 QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
03601 Transform pose;
03602 if(_currentGTPosesMap.size() &&
03603 _ui->actionAnchor_clouds_to_ground_truth->isChecked() &&
03604 _currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end())
03605 {
03606 pose = _currentGTPosesMap.at(nodeId);
03607 }
03608 else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
03609 {
03610 pose = _currentPosesMap.at(nodeId);
03611 }
03612
03613 if(!pose.isNull() || !visible)
03614 {
03615 if(_preferencesDialog->isCloudsShown(0))
03616 {
03617 std::string cloudName = uFormat("cloud%d", nodeId);
03618 if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
03619 {
03620 createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
03621 }
03622 else if(viewerClouds.contains(cloudName))
03623 {
03624 if(visible)
03625 {
03626
03627 _cloudViewer->updateCloudPose(cloudName, pose);
03628 }
03629 _cloudViewer->setCloudVisibility(cloudName, visible);
03630 }
03631 }
03632
03633 if(_preferencesDialog->isScansShown(0))
03634 {
03635 std::string scanName = uFormat("scan%d", nodeId);
03636 if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
03637 {
03638 createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
03639 }
03640 else if(viewerClouds.contains(scanName))
03641 {
03642 if(visible)
03643 {
03644
03645 _cloudViewer->updateCloudPose(scanName, pose);
03646 }
03647 _cloudViewer->setCloudVisibility(scanName, visible);
03648 }
03649 }
03650
03651 _cloudViewer->update();
03652 }
03653 }
03654
03655 void MainWindow::updateGraphView()
03656 {
03657 if(_ui->dockWidget_graphViewer->isVisible())
03658 {
03659 UDEBUG("Graph visible!");
03660 if(_currentPosesMap.size())
03661 {
03662 this->updateMapCloud(
03663 std::map<int, Transform>(_currentPosesMap),
03664 std::multimap<int, Link>(_currentLinksMap),
03665 std::map<int, int>(_currentMapIds),
03666 std::map<int, std::string>(_currentLabels),
03667 std::map<int, Transform>(_currentGTPosesMap));
03668 }
03669 }
03670 }
03671
03672 void MainWindow::processRtabmapEventInit(int status, const QString & info)
03673 {
03674 if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
03675 {
03676 _progressDialog->resetProgress();
03677 _progressDialog->show();
03678 this->changeState(MainWindow::kInitializing);
03679 }
03680 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
03681 {
03682 _progressDialog->setValue(_progressDialog->maximumSteps());
03683 this->changeState(MainWindow::kInitialized);
03684
03685 if(!_openedDatabasePath.isEmpty())
03686 {
03687 this->downloadAllClouds();
03688 }
03689 }
03690 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
03691 {
03692 _progressDialog->resetProgress();
03693 _progressDialog->show();
03694 if(_state!=kApplicationClosing)
03695 {
03696 this->changeState(MainWindow::kClosing);
03697 }
03698 }
03699 else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
03700 {
03701 _progressDialog->setValue(_progressDialog->maximumSteps());
03702
03703 if(_databaseUpdated)
03704 {
03705 if(!_newDatabasePath.isEmpty())
03706 {
03707 if(!_newDatabasePathOutput.isEmpty())
03708 {
03709 bool removed = true;
03710 if(QFile::exists(_newDatabasePathOutput))
03711 {
03712 removed = QFile::remove(_newDatabasePathOutput);
03713 }
03714 if(removed)
03715 {
03716 if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
03717 {
03718 std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
03719 UINFO(msg.c_str());
03720 QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
03721 }
03722 else
03723 {
03724 std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".",
03725 _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
03726 UERROR(msg.c_str());
03727 QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
03728 }
03729 }
03730 else
03731 {
03732 std::string msg = uFormat("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
03733 _newDatabasePathOutput.toStdString().c_str(), _newDatabasePath.toStdString().c_str());
03734 UERROR(msg.c_str());
03735 QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
03736 }
03737 }
03738 else if(QFile::remove(_newDatabasePath))
03739 {
03740 UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
03741 }
03742 else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
03743 {
03744 UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
03745 }
03746
03747 }
03748 else if(!_openedDatabasePath.isEmpty())
03749 {
03750 std::string msg = uFormat("Database \"%s\" updated.", _openedDatabasePath.toStdString().c_str());
03751 UINFO(msg.c_str());
03752 QMessageBox::information(this, tr("Database updated!"), QString(msg.c_str()));
03753 }
03754 }
03755 else if(!_newDatabasePath.isEmpty())
03756 {
03757
03758 if(QFile::remove(_newDatabasePath))
03759 {
03760 UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
03761 }
03762 else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
03763 {
03764 UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
03765 }
03766 }
03767 _openedDatabasePath.clear();
03768 _newDatabasePath.clear();
03769 _newDatabasePathOutput.clear();
03770 bool applicationClosing = _state == kApplicationClosing;
03771 this->changeState(MainWindow::kIdle);
03772 if(applicationClosing)
03773 {
03774 this->close();
03775 }
03776 }
03777 else
03778 {
03779 _progressDialog->incrementStep();
03780 QString msg(info);
03781 if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
03782 {
03783 _openedDatabasePath.clear();
03784 _newDatabasePath.clear();
03785 _newDatabasePathOutput.clear();
03786 _progressDialog->setAutoClose(false);
03787 msg.prepend(tr("[ERROR] "));
03788 _progressDialog->appendText(msg);
03789 this->changeState(MainWindow::kIdle);
03790 }
03791 else
03792 {
03793 _progressDialog->appendText(msg);
03794 }
03795 }
03796 }
03797
03798 void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event)
03799 {
03800 _progressDialog->appendText("Downloading the map... done.");
03801 _progressDialog->incrementStep();
03802
03803 if(event.getCode())
03804 {
03805 UERROR("Map received with code error %d!", event.getCode());
03806 _progressDialog->appendText(uFormat("[ERROR] Map received with code error %d!", event.getCode()).c_str());
03807 _progressDialog->setAutoClose(false);
03808 }
03809 else
03810 {
03811
03812 _processingDownloadedMap = true;
03813 UINFO("Received map!");
03814 _progressDialog->appendText(tr(" poses = %1").arg(event.getPoses().size()));
03815 _progressDialog->appendText(tr(" constraints = %1").arg(event.getConstraints().size()));
03816
03817 _progressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
03818 _progressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
03819 QApplication::processEvents();
03820
03821 int addedSignatures = 0;
03822 std::map<int, int> mapIds;
03823 std::map<int, Transform> groundTruth;
03824 std::map<int, std::string> labels;
03825 for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
03826 iter!=event.getSignatures().end();
03827 ++iter)
03828 {
03829 mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
03830 if(!iter->second.getGroundTruthPose().isNull())
03831 {
03832 groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
03833 }
03834 if(!iter->second.getLabel().empty())
03835 {
03836 labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
03837 }
03838 if(!_cachedSignatures.contains(iter->first) ||
03839 (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty()))
03840 {
03841 _cachedSignatures.insert(iter->first, iter->second);
03842 _cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
03843 unsigned int count = 0;
03844 for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->second.getWords3().upper_bound(-1); jter!=iter->second.getWords3().end(); ++jter)
03845 {
03846 if(util3d::isFinite(jter->second))
03847 {
03848 ++count;
03849 }
03850 }
03851 _cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
03852 ++addedSignatures;
03853 }
03854 _progressDialog->incrementStep();
03855 QApplication::processEvents();
03856 }
03857 _progressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
03858 _progressDialog->incrementStep();
03859 QApplication::processEvents();
03860
03861 _progressDialog->appendText("Inserting data in the cache... done.");
03862
03863 if(event.getPoses().size())
03864 {
03865 _progressDialog->appendText("Updating the 3D map cloud...");
03866 _progressDialog->incrementStep();
03867 _progressDialog->setCancelButtonVisible(true);
03868 _progressCanceled = false;
03869 QApplication::processEvents();
03870 std::map<int, Transform> poses = event.getPoses();
03871 this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
03872
03873 if( _ui->graphicsView_graphView->isVisible() &&
03874 _preferencesDialog->isWordsCountGraphView() &&
03875 _preferencesDialog->isRGBDMode()&&
03876 _cachedWordsCount.size())
03877 {
03878 _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
03879 }
03880
03881 _progressDialog->appendText("Updating the 3D map cloud... done.");
03882 }
03883 else
03884 {
03885 _progressDialog->appendText("No poses received! The map cloud cannot be updated...");
03886 UINFO("Map received is empty! Cannot update the map cloud...");
03887 }
03888
03889 _progressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
03890
03891 if(!_preferencesDialog->isImagesKept())
03892 {
03893 _cachedSignatures.clear();
03894 _cachedMemoryUsage = 0;
03895 _cachedWordsCount.clear();
03896 }
03897 if(_state != kMonitoring && _state != kDetecting)
03898 {
03899 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
03900 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
03901 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
03902 }
03903 _processingDownloadedMap = false;
03904 }
03905 _progressDialog->setValue(_progressDialog->maximumSteps());
03906 _progressDialog->setCancelButtonVisible(false);
03907 _progressCanceled = false;
03908
03909 Q_EMIT(rtabmapEvent3DMapProcessed());
03910 }
03911
03912 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
03913 {
03914 if(!event.getPoses().empty())
03915 {
03916 _ui->graphicsView_graphView->setGlobalPath(event.getPoses());
03917 }
03918
03919 _ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0), _preferencesDialog->isCacheSavedInFigures());
03920 _ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0), _preferencesDialog->isCacheSavedInFigures());
03921 _ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()), _preferencesDialog->isCacheSavedInFigures());
03922 _ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()), _preferencesDialog->isCacheSavedInFigures());
03923 _ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())), _preferencesDialog->isCacheSavedInFigures());
03924
03925 if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
03926 {
03927
03928 if(event.getPoses().empty())
03929 {
03930 QMessageBox * warn = new QMessageBox(
03931 QMessageBox::Warning,
03932 tr("Setting goal failed!"),
03933 tr("Setting goal to location %1%2 failed. "
03934 "Some reasons: \n"
03935 "1) the robot is not yet localized in the map,\n"
03936 "2) the location doesn't exist in the map,\n"
03937 "3) the location is not linked to the global map or\n"
03938 "4) the location is too near of the current location (goal already reached).")
03939 .arg(event.getGoal())
03940 .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):""),
03941 QMessageBox::Ok,
03942 this);
03943 warn->setAttribute(Qt::WA_DeleteOnClose, true);
03944 warn->show();
03945 }
03946 else
03947 {
03948 QMessageBox * info = new QMessageBox(
03949 QMessageBox::Information,
03950 tr("Goal detected!"),
03951 tr("Global path computed to %1%2 (%3 poses, %4 m).")
03952 .arg(event.getGoal())
03953 .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):"")
03954 .arg(event.getPoses().size())
03955 .arg(graph::computePathLength(event.getPoses())),
03956 QMessageBox::Ok,
03957 this);
03958 info->setAttribute(Qt::WA_DeleteOnClose, true);
03959 info->show();
03960 }
03961 }
03962 else if(event.getPoses().empty() && _waypoints.size())
03963 {
03964
03965 uSleep(1000);
03966 this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
03967 }
03968 }
03969
03970 void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
03971 {
03972 QMessageBox * warn = new QMessageBox(
03973 QMessageBox::Warning,
03974 tr("Setting label failed!"),
03975 tr("Setting label %1 to location %2 failed. "
03976 "Some reasons: \n"
03977 "1) the location doesn't exist in the map,\n"
03978 "2) the location has already a label.").arg(label).arg(id),
03979 QMessageBox::Ok,
03980 this);
03981 warn->setAttribute(Qt::WA_DeleteOnClose, true);
03982 warn->show();
03983 }
03984
03985 void MainWindow::processRtabmapGoalStatusEvent(int status)
03986 {
03987 _ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
03988 if(_waypoints.size())
03989 {
03990 this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
03991 }
03992 }
03993
03994 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
03995 {
03996 ULOGGER_DEBUG("");
03997 if(flags & PreferencesDialog::kPanelSource)
03998 {
03999
04000 _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
04001 this->updateSelectSourceMenu();
04002 _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
04003
04004 if(_camera)
04005 {
04006 if(dynamic_cast<DBReader*>(_camera->camera()) != 0)
04007 {
04008 _camera->setImageRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
04009 }
04010 else
04011 {
04012 _camera->setImageRate(_preferencesDialog->getGeneralInputRate());
04013 }
04014 }
04015
04016 }
04017
04018 if(flags & PreferencesDialog::kPanelGeneral)
04019 {
04020 UDEBUG("General settings changed...");
04021 setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
04022 if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
04023 {
04024 _ui->graphicsView_graphView->clearPosterior();
04025 }
04026 }
04027
04028 if(flags & PreferencesDialog::kPanelCloudRendering)
04029 {
04030 UDEBUG("Cloud rendering settings changed...");
04031 if(_currentPosesMap.size())
04032 {
04033 this->updateMapCloud(
04034 std::map<int, Transform>(_currentPosesMap),
04035 std::multimap<int, Link>(_currentLinksMap),
04036 std::map<int, int>(_currentMapIds),
04037 std::map<int, std::string>(_currentLabels),
04038 std::map<int, Transform>(_currentGTPosesMap));
04039 }
04040 }
04041
04042 if(flags & PreferencesDialog::kPanelLogging)
04043 {
04044 UDEBUG("Logging settings changed...");
04045 ULogger::setLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerLevel());
04046 ULogger::setEventLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerEventLevel());
04047 ULogger::setType((ULogger::Type)_preferencesDialog->getGeneralLoggerType(),
04048 (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
04049 ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
04050 ULogger::setPrintThreadId(_preferencesDialog->getGeneralLoggerPrintThreadId());
04051 ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
04052 }
04053 }
04054
04055 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
04056 {
04057 applyPrefSettings(parameters, true);
04058 }
04059
04060 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
04061 {
04062 ULOGGER_DEBUG("");
04063 _occupancyGrid->parseParameters(parameters);
04064 if(parameters.size())
04065 {
04066 for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
04067 {
04068 UDEBUG("Parameter changed: Key=%s Value=%s", iter->first.c_str(), iter->second.c_str());
04069 }
04070
04071 rtabmap::ParametersMap parametersModified = parameters;
04072
04073 if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
04074 {
04075 _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
04076 _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
04077 _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
04078 }
04079
04080 if(_state != kIdle && parametersModified.size())
04081 {
04082 if(postParamEvent)
04083 {
04084 this->post(new ParamEvent(parametersModified));
04085 }
04086 }
04087
04088
04089 _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
04090 _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
04091
04092
04093 if(uContains(parameters, Parameters::kRGBDLocalRadius()))
04094 {
04095 _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
04096 }
04097 }
04098
04099
04100 _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
04101 _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
04102 _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
04103
04104 float value;
04105 value = float(_preferencesDialog->getLoopThr());
04106 Q_EMIT(loopClosureThrChanged(value));
04107 }
04108
04109 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
04110 {
04111 UTimer timer;
04112
04113 timer.start();
04114 ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
04115 if(refWords.size())
04116 {
04117 _ui->imageView_source->clearFeatures();
04118 }
04119 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
04120 {
04121 int id = iter->first;
04122 QColor color;
04123 if(id<0)
04124 {
04125
04126 color = Qt::gray;
04127 }
04128 else if(uContains(loopWords, id))
04129 {
04130
04131 color = Qt::magenta;
04132 }
04133 else if(_lastIds.contains(id))
04134 {
04135
04136 color = Qt::blue;
04137 }
04138 else if(id<=_lastId)
04139 {
04140
04141 color = Qt::red;
04142 }
04143 else if(refWords.count(id) > 1)
04144 {
04145
04146 color = Qt::yellow;
04147 }
04148 else
04149 {
04150
04151 color = Qt::green;
04152 }
04153 _ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
04154 }
04155 ULOGGER_DEBUG("source time = %f s", timer.ticks());
04156
04157 timer.start();
04158 ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
04159 QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
04160 if(loopWords.size())
04161 {
04162 _ui->imageView_loopClosure->clearFeatures();
04163 }
04164 for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
04165 {
04166 int id = iter->first;
04167 QColor color;
04168 if(id<0)
04169 {
04170
04171 color = Qt::gray;
04172 }
04173 else if(uContains(refWords, id))
04174 {
04175
04176 color = Qt::magenta;
04177
04178 if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
04179 {
04180 const cv::KeyPoint & a = refWords.find(id)->second;
04181 const cv::KeyPoint & b = iter->second;
04182 uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
04183 }
04184 }
04185 else if(id<=_lastId)
04186 {
04187
04188 color = Qt::red;
04189 }
04190 else if(refWords.count(id) > 1)
04191 {
04192
04193 color = Qt::yellow;
04194 }
04195 else
04196 {
04197
04198 color = Qt::green;
04199 }
04200 _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
04201 }
04202
04203 ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
04204
04205 if(refWords.size()>0)
04206 {
04207 if((*refWords.rbegin()).first > _lastId)
04208 {
04209 _lastId = (*refWords.rbegin()).first;
04210 }
04211 _lastIds = QSet<int>::fromList(QList<int>::fromStdList(uKeysList(refWords)));
04212 }
04213
04214
04215 float scaleSource = _ui->imageView_source->viewScale();
04216 float scaleLoop = _ui->imageView_loopClosure->viewScale();
04217 UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
04218
04219 float sourceMarginX = (_ui->imageView_source->width() - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
04220 float sourceMarginY = (_ui->imageView_source->height() - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
04221 float loopMarginX = (_ui->imageView_loopClosure->width() - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
04222 float loopMarginY = (_ui->imageView_loopClosure->height() - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
04223
04224 float deltaX = 0;
04225 float deltaY = 0;
04226
04227 if(_preferencesDialog->isVerticalLayoutUsed())
04228 {
04229 deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
04230 }
04231 else
04232 {
04233 deltaX = _ui->imageView_source->width();
04234 }
04235
04236 if(refWords.size() && loopWords.size())
04237 {
04238 _ui->imageView_source->clearLines();
04239 _ui->imageView_loopClosure->clearLines();
04240 }
04241
04242 for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
04243 iter!=uniqueCorrespondences.end();
04244 ++iter)
04245 {
04246
04247 _ui->imageView_source->addLine(
04248 iter->first.x,
04249 iter->first.y,
04250 (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
04251 (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
04252 Qt::cyan);
04253
04254 _ui->imageView_loopClosure->addLine(
04255 (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
04256 (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
04257 iter->second.x,
04258 iter->second.y,
04259 Qt::cyan);
04260 }
04261 _ui->imageView_source->update();
04262 _ui->imageView_loopClosure->update();
04263 }
04264
04265 void MainWindow::showEvent(QShowEvent* anEvent)
04266 {
04267
04268 this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
04269 }
04270
04271 void MainWindow::moveEvent(QMoveEvent* anEvent)
04272 {
04273 if(this->isVisible())
04274 {
04275
04276 if(!_firstCall)
04277 {
04278 this->configGUIModified();
04279 }
04280 _firstCall = false;
04281 }
04282 }
04283
04284 void MainWindow::resizeEvent(QResizeEvent* anEvent)
04285 {
04286 if(this->isVisible())
04287 {
04288 this->configGUIModified();
04289 }
04290 }
04291
04292 void MainWindow::keyPressEvent(QKeyEvent *event)
04293 {
04294
04295 if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
04296 {
04297 this->saveConfigGUI();
04298 }
04299 }
04300
04301 bool MainWindow::eventFilter(QObject *obj, QEvent *event)
04302 {
04303 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
04304 {
04305 this->setWindowModified(true);
04306 }
04307 else if(event->type() == QEvent::FileOpen )
04308 {
04309 openDatabase(((QFileOpenEvent*)event)->file());
04310 }
04311 return QWidget::eventFilter(obj, event);
04312 }
04313
04314 void MainWindow::updateSelectSourceMenu()
04315 {
04316 _ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice);
04317
04318 _ui->actionMore_options->setChecked(
04319 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
04320 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
04321 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
04322 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
04323 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
04324 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages
04325 );
04326
04327 _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
04328 _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
04329 _ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
04330 _ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
04331 _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
04332 _ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
04333 _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
04334 _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
04335 _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
04336 _ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
04337 _ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
04338 _ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
04339 _ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
04340 _ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
04341 _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
04342 _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
04343 _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
04344 _ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
04345 }
04346
04347 void MainWindow::changeImgRateSetting()
04348 {
04349 Q_EMIT imgRateChanged(_ui->doubleSpinBox_stats_imgRate->value());
04350 }
04351
04352 void MainWindow::changeDetectionRateSetting()
04353 {
04354 Q_EMIT detectionRateChanged(_ui->doubleSpinBox_stats_detectionRate->value());
04355 }
04356
04357 void MainWindow::changeTimeLimitSetting()
04358 {
04359 Q_EMIT timeLimitChanged((float)_ui->doubleSpinBox_stats_timeLimit->value());
04360 }
04361
04362 void MainWindow::changeMappingMode()
04363 {
04364 Q_EMIT mappingModeChanged(_ui->actionSLAM_mode->isChecked());
04365 }
04366
04367 QString MainWindow::captureScreen(bool cacheInRAM, bool png)
04368 {
04369 QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (png?".png":".jpg"));
04370 _ui->statusbar->clearMessage();
04371 QPixmap figure = QPixmap::grabWidget(this);
04372
04373 QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
04374 QString msg;
04375 if(cacheInRAM)
04376 {
04377 msg = tr("Screen captured \"%1\"").arg(name);
04378 QByteArray bytes;
04379 QBuffer buffer(&bytes);
04380 buffer.open(QIODevice::WriteOnly);
04381 figure.save(&buffer, png?"PNG":"JPEG");
04382 _autoScreenCaptureCachedImages.insert(name, bytes);
04383 }
04384 else
04385 {
04386 QDir dir;
04387 if(!dir.exists(targetDir))
04388 {
04389 dir.mkdir(targetDir);
04390 }
04391 targetDir += QDir::separator();
04392 targetDir += "Main_window";
04393 if(!dir.exists(targetDir))
04394 {
04395 dir.mkdir(targetDir);
04396 }
04397 targetDir += QDir::separator();
04398
04399 figure.save(targetDir + name);
04400 msg = tr("Screen captured \"%1\"").arg(targetDir + name);
04401 }
04402 _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
04403 _ui->widget_console->appendMsg(msg);
04404
04405 return targetDir + name;
04406 }
04407
04408 void MainWindow::beep()
04409 {
04410 QApplication::beep();
04411 }
04412
04413 void MainWindow::cancelProgress()
04414 {
04415 _progressCanceled = true;
04416 _progressDialog->appendText(tr("Canceled!"));
04417 }
04418
04419 void MainWindow::configGUIModified()
04420 {
04421 this->setWindowModified(true);
04422 }
04423
04424 void MainWindow::updateParameters(const ParametersMap & parameters)
04425 {
04426 if(parameters.size())
04427 {
04428 for(ParametersMap::const_iterator iter= parameters.begin(); iter!=parameters.end(); ++iter)
04429 {
04430 QString msg = tr("Parameter update \"%1\"=\"%2\"")
04431 .arg(iter->first.c_str())
04432 .arg(iter->second.c_str());
04433 _ui->widget_console->appendMsg(msg);
04434 UWARN(msg.toStdString().c_str());
04435 }
04436 QMessageBox::StandardButton button = QMessageBox::question(this,
04437 tr("Parameters"),
04438 tr("Some parameters have been set on command line, do you "
04439 "want to set all other RTAB-Map's parameters to default?"),
04440 QMessageBox::Yes | QMessageBox::No,
04441 QMessageBox::No);
04442 _preferencesDialog->updateParameters(parameters, button==QMessageBox::Yes);
04443 }
04444 }
04445
04446
04447 void MainWindow::saveConfigGUI()
04448 {
04449 _savedMaximized = this->isMaximized();
04450 _preferencesDialog->saveMainWindowState(this);
04451 _preferencesDialog->saveWindowGeometry(_preferencesDialog);
04452 _preferencesDialog->saveWindowGeometry(_aboutDialog);
04453 _preferencesDialog->saveWidgetState(_cloudViewer);
04454 _preferencesDialog->saveWidgetState(_ui->imageView_source);
04455 _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
04456 _preferencesDialog->saveWidgetState(_ui->imageView_odometry);
04457 _preferencesDialog->saveWidgetState(_exportCloudsDialog);
04458 _preferencesDialog->saveWidgetState(_exportBundlerDialog);
04459 _preferencesDialog->saveWidgetState(_postProcessingDialog);
04460 _preferencesDialog->saveWidgetState(_depthCalibrationDialog);
04461 _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
04462 _preferencesDialog->saveSettings();
04463 this->saveFigures();
04464 this->setWindowModified(false);
04465 }
04466
04467 void MainWindow::newDatabase()
04468 {
04469 if(_state != MainWindow::kIdle)
04470 {
04471 UERROR("This method can be called only in IDLE state.");
04472 return;
04473 }
04474 _openedDatabasePath.clear();
04475 _newDatabasePath.clear();
04476 _newDatabasePathOutput.clear();
04477 _databaseUpdated = false;
04478 ULOGGER_DEBUG("");
04479 this->clearTheCache();
04480 std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
04481 if(QFile::exists(databasePath.c_str()))
04482 {
04483 int r = QMessageBox::question(this,
04484 tr("Creating temporary database"),
04485 tr("Cannot create a new database because the temporary database \"%1\" already exists. "
04486 "There may be another instance of RTAB-Map running with the same Working Directory or "
04487 "the last time RTAB-Map was not closed correctly. "
04488 "Do you want to recover the database (click Ignore to delete it and create a new one)?").arg(databasePath.c_str()),
04489 QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::No);
04490
04491 if(r == QMessageBox::Ignore)
04492 {
04493 if(QFile::remove(databasePath.c_str()))
04494 {
04495 UINFO("Deleted temporary database \"%s\".", databasePath.c_str());
04496 }
04497 else
04498 {
04499 UERROR("Temporary database \"%s\" could not be deleted!", databasePath.c_str());
04500 return;
04501 }
04502 }
04503 else if(r == QMessageBox::Yes)
04504 {
04505 std::string errorMsg;
04506 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
04507 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
04508 progressDialog->setMaximumSteps(100);
04509 progressDialog->show();
04510 progressDialog->setCancelButtonVisible(true);
04511 RecoveryState state(progressDialog);
04512 _recovering = true;
04513 if(databaseRecovery(databasePath, false, &errorMsg, &state))
04514 {
04515 _recovering = false;
04516 progressDialog->setValue(progressDialog->maximumSteps());
04517 QString newPath = QFileDialog::getSaveFileName(this, tr("Save recovered database"), _preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("recovered.db"), tr("RTAB-Map database files (*.db)"));
04518 if(newPath.isEmpty())
04519 {
04520 return;
04521 }
04522 if(QFileInfo(newPath).suffix() == "")
04523 {
04524 newPath += ".db";
04525 }
04526 if(QFile::exists(newPath))
04527 {
04528 QFile::remove(newPath);
04529 }
04530 QFile::rename(databasePath.c_str(), newPath);
04531 return;
04532 }
04533 else
04534 {
04535 _recovering = false;
04536 progressDialog->setValue(progressDialog->maximumSteps());
04537 QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
04538 return;
04539 }
04540 }
04541 else
04542 {
04543 return;
04544 }
04545 }
04546 _newDatabasePath = databasePath.c_str();
04547 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
04548 applyPrefSettings(_preferencesDialog->getAllParameters(), false);
04549 }
04550
04551 void MainWindow::openDatabase()
04552 {
04553 QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
04554 if(!path.isEmpty())
04555 {
04556 this->openDatabase(path);
04557 }
04558 }
04559
04560 void MainWindow::openDatabase(const QString & path, const ParametersMap & overridedParameters)
04561 {
04562 if(_state != MainWindow::kIdle)
04563 {
04564 UERROR("Database can only be opened in IDLE state.");
04565 return;
04566 }
04567
04568 std::string value = path.toStdString();
04569 if(UFile::exists(value) &&
04570 UFile::getExtension(value).compare("db") == 0)
04571 {
04572 _openedDatabasePath.clear();
04573 _newDatabasePath.clear();
04574 _newDatabasePathOutput.clear();
04575 _databaseUpdated = false;
04576
04577 this->clearTheCache();
04578 _openedDatabasePath = path;
04579
04580
04581 DBDriver * driver = DBDriver::create();
04582 if(driver->openConnection(value, false))
04583 {
04584 ParametersMap parameters = driver->getLastParameters();
04585 driver->closeConnection(false);
04586 delete driver;
04587
04588 if(parameters.size())
04589 {
04590
04591 for(ParametersMap::const_iterator iter=Parameters::getDefaultParameters().begin(); iter!=Parameters::getDefaultParameters().end(); ++iter)
04592 {
04593 parameters.insert(*iter);
04594 }
04595
04596 uInsert(parameters, overridedParameters);
04597
04598 ParametersMap currentParameters = _preferencesDialog->getAllParameters();
04599 ParametersMap differentParameters;
04600 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
04601 {
04602 ParametersMap::iterator jter = currentParameters.find(iter->first);
04603 if(jter!=currentParameters.end() &&
04604 iter->second.compare(jter->second) != 0 &&
04605 iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
04606 {
04607 bool different = true;
04608 if(Parameters::getType(iter->first).compare("double") ==0 ||
04609 Parameters::getType(iter->first).compare("float") == 0)
04610 {
04611 if(uStr2Double(iter->second) == uStr2Double(jter->second))
04612 {
04613 different = false;
04614 }
04615 }
04616 if(different)
04617 {
04618 differentParameters.insert(*iter);
04619 QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
04620 .arg(iter->first.c_str())
04621 .arg(iter->second.c_str())
04622 .arg(jter->second.c_str());
04623 _ui->widget_console->appendMsg(msg);
04624 UWARN(msg.toStdString().c_str());
04625 }
04626 }
04627 }
04628
04629 if(differentParameters.size())
04630 {
04631 int r = QMessageBox::question(this,
04632 tr("Update parameters..."),
04633 tr("The database is using %1 different parameter(s) than "
04634 "those currently set in Preferences. Do you want "
04635 "to use database's parameters?").arg(differentParameters.size()),
04636 QMessageBox::Yes | QMessageBox::No,
04637 QMessageBox::Yes);
04638 if(r == QMessageBox::Yes)
04639 {
04640 _preferencesDialog->updateParameters(differentParameters);
04641 }
04642 }
04643 }
04644 }
04645
04646 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, value, 0, _preferencesDialog->getAllParameters()));
04647 applyPrefSettings(_preferencesDialog->getAllParameters(), false);
04648 }
04649 else
04650 {
04651 UERROR("File \"%s\" not valid.", value.c_str());
04652 }
04653 }
04654
04655 bool MainWindow::closeDatabase()
04656 {
04657 if(_state != MainWindow::kInitialized)
04658 {
04659 UERROR("This method can be called only in INITIALIZED state.");
04660 return false;
04661 }
04662
04663 _newDatabasePathOutput.clear();
04664 if(!_newDatabasePath.isEmpty() && _databaseUpdated)
04665 {
04666 QMessageBox::Button b = QMessageBox::question(this,
04667 tr("Save database"),
04668 tr("Save the new database?"),
04669 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard,
04670 QMessageBox::Save);
04671
04672 if(b == QMessageBox::Save)
04673 {
04674
04675 QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
04676 QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
04677
04678 newPath = QFileDialog::getSaveFileName(this, tr("Save database"), newPath, tr("RTAB-Map database files (*.db)"));
04679 if(newPath.isEmpty())
04680 {
04681 return false;
04682 }
04683
04684 if(QFileInfo(newPath).suffix() == "")
04685 {
04686 newPath += ".db";
04687 }
04688
04689 _newDatabasePathOutput = newPath;
04690 }
04691 else if(b != QMessageBox::Discard)
04692 {
04693 return false;
04694 }
04695 }
04696
04697 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_openedDatabasePath.isEmpty() || !_newDatabasePathOutput.isEmpty()));
04698 return true;
04699 }
04700
04701 void MainWindow::editDatabase()
04702 {
04703 if(_state != MainWindow::kIdle)
04704 {
04705 UERROR("This method can be called only in IDLE state.");
04706 return;
04707 }
04708 QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
04709 if(!path.isEmpty())
04710 {
04711 {
04712
04713 QSettings dbSettingsIn(_preferencesDialog->getIniFilePath(), QSettings::IniFormat);
04714 QSettings dbSettingsOut(_preferencesDialog->getTmpIniFilePath(), QSettings::IniFormat);
04715 dbSettingsIn.beginGroup("DatabaseViewer");
04716 dbSettingsOut.beginGroup("DatabaseViewer");
04717 QStringList keys = dbSettingsIn.childKeys();
04718 for(QStringList::iterator iter = keys.begin(); iter!=keys.end(); ++iter)
04719 {
04720 dbSettingsOut.setValue(*iter, dbSettingsIn.value(*iter));
04721 }
04722 dbSettingsIn.endGroup();
04723 dbSettingsOut.endGroup();
04724 }
04725
04726 DatabaseViewer * viewer = new DatabaseViewer(_preferencesDialog->getTmpIniFilePath(), this);
04727 viewer->setWindowModality(Qt::WindowModal);
04728 viewer->setAttribute(Qt::WA_DeleteOnClose, true);
04729 viewer->showCloseButton();
04730
04731 if(viewer->isSavedMaximized())
04732 {
04733 viewer->showMaximized();
04734 }
04735 else
04736 {
04737 viewer->show();
04738 }
04739
04740 viewer->openDatabase(path);
04741 }
04742 }
04743
04744 void MainWindow::startDetection()
04745 {
04746 UDEBUG("");
04747 ParametersMap parameters = _preferencesDialog->getAllParameters();
04748
04749 if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
04750 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
04751 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
04752 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
04753 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
04754 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
04755 {
04756 float inputRate = _preferencesDialog->getGeneralInputRate();
04757 float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
04758 int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
04759 if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) &&
04760 (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed()))
04761 {
04762 int button = QMessageBox::question(this,
04763 tr("Incompatible frame rates!"),
04764 tr("\"Source/Input rate\" (%1 Hz) is equal to/higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the "
04765 "source input is a directory of images/video/database, some images may be "
04766 "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over "
04767 "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to "
04768 "start the detection anyway?").arg(inputRate).arg(detectionRate),
04769 QMessageBox::Yes | QMessageBox::No);
04770 if(button == QMessageBox::No)
04771 {
04772 return;
04773 }
04774 }
04775 if(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed())
04776 {
04777 if(bufferingSize != 0)
04778 {
04779 int button = QMessageBox::question(this,
04780 tr("Some images may be skipped!"),
04781 tr("\"RTAB-Map/Images buffer size\" is not infinite (size=%1). As the "
04782 "source input is a directory of images/video/database, some images may be "
04783 "skipped by the detector if the \"Source/Input rate\" (which is %2 Hz) is higher than the "
04784 "rate at which RTAB-Map can process the images. You may want to set the "
04785 "\"RTAB-Map/Images buffer size\" to 0 (infinite) to guaranty that all "
04786 "images are processed. Would you want to start the detection "
04787 "anyway?").arg(bufferingSize).arg(inputRate),
04788 QMessageBox::Yes | QMessageBox::No);
04789 if(button == QMessageBox::No)
04790 {
04791 return;
04792 }
04793 }
04794 else if(inputRate == 0)
04795 {
04796 int button = QMessageBox::question(this,
04797 tr("Large number of images may be buffered!"),
04798 tr("\"RTAB-Map/Images buffer size\" is infinite. As the "
04799 "source input is a directory of images/video/database and "
04800 "that \"Source/Input rate\" is infinite too, a lot of images "
04801 "could be buffered at the same time (e.g., reading all images "
04802 "of a directory at once). This could make the GUI not responsive. "
04803 "You may want to set \"Source/Input rate\" at the rate at "
04804 "which the images have been recorded. "
04805 "Would you want to start the detection "
04806 "anyway?").arg(bufferingSize).arg(inputRate),
04807 QMessageBox::Yes | QMessageBox::No);
04808 if(button == QMessageBox::No)
04809 {
04810 return;
04811 }
04812 }
04813 }
04814 }
04815
04816 UDEBUG("");
04817 Q_EMIT stateChanged(kStartingDetection);
04818
04819 if(_camera != 0)
04820 {
04821 QMessageBox::warning(this,
04822 tr("RTAB-Map"),
04823 tr("A camera is running, stop it first."));
04824 UWARN("_camera is not null... it must be stopped first");
04825 Q_EMIT stateChanged(kInitialized);
04826 return;
04827 }
04828
04829
04830 if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
04831 {
04832 QMessageBox::warning(this,
04833 tr("RTAB-Map"),
04834 tr("No sources are selected. See Preferences->Source panel."));
04835 UWARN("No sources are selected. See Preferences->Source panel.");
04836 Q_EMIT stateChanged(kInitialized);
04837 return;
04838 }
04839
04840
04841 Camera * camera = _preferencesDialog->createCamera();
04842 if(!camera)
04843 {
04844 Q_EMIT stateChanged(kInitialized);
04845 return;
04846 }
04847
04848 _camera = new CameraThread(camera, parameters);
04849 _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
04850 _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
04851 _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
04852 _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
04853 _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation());
04854 _camera->setScanFromDepth(
04855 _preferencesDialog->isSourceScanFromDepth(),
04856 _preferencesDialog->getSourceScanFromDepthDecimation(),
04857 _preferencesDialog->getSourceScanFromDepthMaxDepth(),
04858 _preferencesDialog->getSourceScanVoxelSize(),
04859 _preferencesDialog->getSourceScanNormalsK(),
04860 _preferencesDialog->getSourceScanNormalsRadius());
04861 if(_preferencesDialog->isDepthFilteringAvailable())
04862 {
04863 if(_preferencesDialog->isBilateralFiltering())
04864 {
04865 _camera->enableBilateralFiltering(
04866 _preferencesDialog->getBilateralSigmaS(),
04867 _preferencesDialog->getBilateralSigmaR());
04868 }
04869 _camera->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString());
04870 }
04871
04872
04873 if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
04874 {
04875
04876 if(!camera->isCalibrated())
04877 {
04878 UWARN("Camera is not calibrated!");
04879 Q_EMIT stateChanged(kInitialized);
04880 delete _camera;
04881 _camera = 0;
04882
04883 int button = QMessageBox::question(this,
04884 tr("Camera is not calibrated!"),
04885 tr("RTAB-Map in metric SLAM mode cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
04886 QMessageBox::Yes | QMessageBox::No);
04887 if(button == QMessageBox::Yes)
04888 {
04889 QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
04890 }
04891 return;
04892 }
04893 else
04894 {
04895 if(_odomThread)
04896 {
04897 UERROR("OdomThread must be already deleted here?!");
04898 delete _odomThread;
04899 _odomThread = 0;
04900 }
04901
04902 if(_imuThread)
04903 {
04904 UERROR("ImuThread must be already deleted here?!");
04905 delete _imuThread;
04906 _imuThread = 0;
04907 }
04908
04909 if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
04910 {
04911 ParametersMap odomParameters = parameters;
04912 if(_preferencesDialog->getOdomRegistrationApproach() < 3)
04913 {
04914 uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
04915 }
04916 odomParameters.erase(Parameters::kRtabmapPublishRAMUsage());
04917 int odomStrategy = Parameters::defaultOdomStrategy();
04918 Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
04919 if(odomStrategy == 1)
04920 {
04921
04922 odomParameters.insert(ParametersPair(Parameters::kVisCorType(), _preferencesDialog->getParameter(Parameters::kVisCorType())));
04923 }
04924 _imuThread = 0;
04925 if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
04926 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
04927 _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
04928 !_preferencesDialog->getIMUPath().isEmpty())
04929 {
04930 if(odomStrategy != Odometry::kTypeOkvis && odomStrategy != Odometry::kTypeMSCKF)
04931 {
04932 QMessageBox::warning(this, tr("Source IMU Path"),
04933 tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);
04934 }
04935 else
04936 {
04937 _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
04938 if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
04939 {
04940 QMessageBox::warning(this, tr("Source IMU Path"),
04941 tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
04942 delete _camera;
04943 _camera = 0;
04944 delete _imuThread;
04945 _imuThread = 0;
04946 return;
04947 }
04948 }
04949 }
04950 Odometry * odom = Odometry::create(odomParameters);
04951 _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
04952
04953 UEventsManager::addHandler(_odomThread);
04954 UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
04955 UEventsManager::createPipe(_camera, this, "CameraEvent");
04956 if(_imuThread)
04957 {
04958 UEventsManager::createPipe(_imuThread, _odomThread, "IMUEvent");
04959 }
04960 _odomThread->start();
04961 }
04962 }
04963 }
04964
04965 if(_dataRecorder && _camera && _odomThread)
04966 {
04967 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
04968 }
04969
04970 _lastOdomPose.setNull();
04971 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer));
04972 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
04973
04974 if(_odomThread)
04975 {
04976 _ui->actionReset_Odometry->setEnabled(true);
04977 }
04978
04979 if(!_preferencesDialog->isStatisticsPublished())
04980 {
04981 QMessageBox::information(this,
04982 tr("Information"),
04983 tr("Note that publishing statistics is disabled, "
04984 "progress will not be shown in the GUI."));
04985 }
04986
04987 _occupancyGrid->clear();
04988 _occupancyGrid->parseParameters(parameters);
04989
04990 #ifdef RTABMAP_OCTOMAP
04991 UASSERT(_octomap != 0);
04992 delete _octomap;
04993 _octomap = new OctoMap(parameters);
04994 #endif
04995
04996
04997 _cloudViewer->removeCloud("cloudOdom");
04998 _cloudViewer->removeCloud("scanOdom");
04999 _cloudViewer->removeCloud("scanMapOdom");
05000 _cloudViewer->removeCloud("featuresOdom");
05001 _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
05002
05003 Q_EMIT stateChanged(kDetecting);
05004 }
05005
05006
05007 void MainWindow::pauseDetection()
05008 {
05009 if(_camera)
05010 {
05011 if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
05012 {
05013
05014 Q_EMIT stateChanged(kPaused);
05015 if(_preferencesDialog->getGeneralInputRate())
05016 {
05017 QTimer::singleShot(500.0/_preferencesDialog->getGeneralInputRate(), this, SLOT(pauseDetection()));
05018 }
05019 else
05020 {
05021 Q_EMIT stateChanged(kPaused);
05022 }
05023 }
05024 else
05025 {
05026 Q_EMIT stateChanged(kPaused);
05027 }
05028 }
05029 else if(_state == kMonitoring)
05030 {
05031 UINFO("Sending pause event!");
05032 Q_EMIT stateChanged(kMonitoringPaused);
05033 }
05034 else if(_state == kMonitoringPaused)
05035 {
05036 UINFO("Sending unpause event!");
05037 Q_EMIT stateChanged(kMonitoring);
05038 }
05039 }
05040
05041 void MainWindow::stopDetection()
05042 {
05043 if(!_camera && !_odomThread)
05044 {
05045 return;
05046 }
05047
05048 if(_state == kDetecting &&
05049 (_camera && _camera->isRunning()) )
05050 {
05051 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);
05052
05053 if(button != QMessageBox::Yes)
05054 {
05055 return;
05056 }
05057 }
05058
05059 ULOGGER_DEBUG("");
05060
05061 if(_imuThread)
05062 {
05063 _imuThread->join(true);
05064 }
05065
05066 if(_camera)
05067 {
05068 _camera->join(true);
05069 }
05070
05071 if(_odomThread)
05072 {
05073 _ui->actionReset_Odometry->setEnabled(false);
05074 _odomThread->kill();
05075 }
05076
05077
05078 if(_imuThread)
05079 {
05080 delete _imuThread;
05081 _imuThread = 0;
05082 }
05083 if(_camera)
05084 {
05085 delete _camera;
05086 _camera = 0;
05087 }
05088 if(_odomThread)
05089 {
05090 delete _odomThread;
05091 _odomThread = 0;
05092 }
05093
05094 if(_dataRecorder)
05095 {
05096 delete _dataRecorder;
05097 _dataRecorder = 0;
05098 }
05099
05100 Q_EMIT stateChanged(kInitialized);
05101 }
05102
05103 void MainWindow::notifyNoMoreImages()
05104 {
05105 QMessageBox::information(this,
05106 tr("No more images..."),
05107 tr("The camera has reached the end of the stream."));
05108 }
05109
05110 void MainWindow::printLoopClosureIds()
05111 {
05112 _ui->dockWidget_console->show();
05113 QString msgRef;
05114 QString msgLoop;
05115 for(int i = 0; i<_refIds.size(); ++i)
05116 {
05117 msgRef.append(QString::number(_refIds[i]));
05118 msgLoop.append(QString::number(_loopClosureIds[i]));
05119 if(i < _refIds.size() - 1)
05120 {
05121 msgRef.append(" ");
05122 msgLoop.append(" ");
05123 }
05124 }
05125 _ui->widget_console->appendMsg(QString("IDs = [%1];").arg(msgRef));
05126 _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
05127 }
05128
05129 void MainWindow::generateGraphDOT()
05130 {
05131 if(_graphSavingFileName.isEmpty())
05132 {
05133 _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
05134 }
05135
05136 bool ok;
05137 int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
05138 if(ok)
05139 {
05140 int margin = 0;
05141 if(id > 0)
05142 {
05143 margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
05144 }
05145
05146 if(ok)
05147 {
05148 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
05149 if(!path.isEmpty())
05150 {
05151 _graphSavingFileName = path;
05152 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
05153
05154 _ui->dockWidget_console->show();
05155 _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName));
05156 }
05157 }
05158 }
05159 }
05160
05161 void MainWindow::exportPosesRaw()
05162 {
05163 exportPoses(0);
05164 }
05165 void MainWindow::exportPosesRGBDSLAM()
05166 {
05167 exportPoses(1);
05168 }
05169 void MainWindow::exportPosesKITTI()
05170 {
05171 exportPoses(2);
05172 }
05173 void MainWindow::exportPosesTORO()
05174 {
05175 exportPoses(3);
05176 }
05177 void MainWindow::exportPosesG2O()
05178 {
05179 exportPoses(4);
05180 }
05181
05182 void MainWindow::exportPoses(int format)
05183 {
05184 if(_currentPosesMap.size())
05185 {
05186 std::map<int, Transform> localTransforms;
05187 QStringList items;
05188 items.push_back("Robot");
05189 items.push_back("Camera");
05190 items.push_back("Scan");
05191 bool ok;
05192 QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false, &ok);
05193 if(!ok || item.isEmpty())
05194 {
05195 return;
05196 }
05197 if(item.compare("Robot") != 0)
05198 {
05199 bool cameraFrame = item.compare("Camera") == 0;
05200 _exportPosesFrame = cameraFrame?1:2;
05201 for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
05202 {
05203 if(_cachedSignatures.contains(iter->first))
05204 {
05205 Transform localTransform;
05206 if(cameraFrame)
05207 {
05208 if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
05209 !_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull()))
05210 {
05211 localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
05212 }
05213 else if(!_cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform().isNull())
05214 {
05215 localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform();
05216 }
05217 else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1)
05218 {
05219 UWARN("Multi-camera is not supported (node %d)", iter->first);
05220 }
05221 else
05222 {
05223 UWARN("Missing calibration for node %d", iter->first);
05224 }
05225 }
05226 else
05227 {
05228 if(!_cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform().isNull())
05229 {
05230 localTransform = _cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform();
05231 }
05232 else if(!_cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform().isNull())
05233 {
05234 localTransform = _cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform();
05235 }
05236 else
05237 {
05238 UWARN("Missing scan info for node %d", iter->first);
05239 }
05240 }
05241 if(!localTransform.isNull())
05242 {
05243 localTransforms.insert(std::make_pair(iter->first, localTransform));
05244 }
05245 }
05246 else
05247 {
05248 UWARN("Did not find node %d in cache", iter->first);
05249 }
05250 }
05251 if(localTransforms.empty())
05252 {
05253 QMessageBox::warning(this,
05254 tr("Export Poses"),
05255 tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
05256 }
05257 }
05258 else
05259 {
05260 _exportPosesFrame = 0;
05261 }
05262
05263 std::map<int, Transform> poses;
05264 std::multimap<int, Link> links;
05265 if(localTransforms.empty())
05266 {
05267 poses = _currentPosesMap;
05268 links = _currentLinksMap;
05269 }
05270 else
05271 {
05272
05273 for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
05274 {
05275 poses.insert(std::make_pair(iter->first, _currentPosesMap.at(iter->first) * iter->second));
05276 }
05277 for(std::multimap<int, Link>::iterator iter=_currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter)
05278 {
05279 if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
05280 {
05281 std::multimap<int, Link>::iterator inserted = links.insert(*iter);
05282 int from = iter->second.from();
05283 int to = iter->second.to();
05284 inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
05285 }
05286 }
05287 }
05288
05289 std::map<int, double> stamps;
05290 if(format == 1)
05291 {
05292 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
05293 {
05294 if(_cachedSignatures.contains(iter->first))
05295 {
05296 stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
05297 }
05298 }
05299 if(stamps.size()!=poses.size())
05300 {
05301 QMessageBox::warning(this, tr("Export poses..."), tr("RGB-D SLAM format: Poses (%1) and stamps (%2) have not the same size! Try again after updating the cache.")
05302 .arg(poses.size()).arg(stamps.size()));
05303 return;
05304 }
05305 }
05306
05307 if(_exportPosesFileName[format].isEmpty())
05308 {
05309 _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
05310 }
05311
05312 QString path = QFileDialog::getSaveFileName(
05313 this,
05314 tr("Save File"),
05315 _exportPosesFileName[format],
05316 format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
05317
05318 if(!path.isEmpty())
05319 {
05320 if(QFileInfo(path).suffix() == "")
05321 {
05322 if(format == 3)
05323 {
05324 path += ".graph";
05325 }
05326 else if(format==4)
05327 {
05328 path += ".g2o";
05329 }
05330 else
05331 {
05332 path += ".txt";
05333 }
05334 }
05335
05336 _exportPosesFileName[format] = path;
05337 bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
05338
05339 if(saved)
05340 {
05341 QMessageBox::information(this,
05342 tr("Export poses..."),
05343 tr("%1 saved to \"%2\".")
05344 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
05345 .arg(_exportPosesFileName[format]));
05346 }
05347 else
05348 {
05349 QMessageBox::information(this,
05350 tr("Export poses..."),
05351 tr("Failed to save %1 to \"%2\"!")
05352 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
05353 .arg(_exportPosesFileName[format]));
05354 }
05355 }
05356 }
05357 }
05358
05359 void MainWindow::postProcessing()
05360 {
05361 if(_cachedSignatures.size() == 0)
05362 {
05363 QMessageBox::warning(this,
05364 tr("Post-processing..."),
05365 tr("Signatures must be cached in the GUI for post-processing. "
05366 "Check the option in Preferences->General Settings (GUI), then "
05367 "refresh the cache."));
05368 return;
05369 }
05370 if(_postProcessingDialog->exec() != QDialog::Accepted)
05371 {
05372 return;
05373 }
05374
05375 bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
05376 bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
05377 bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
05378 double clusterRadius = _postProcessingDialog->clusterRadius();
05379 double clusterAngle = _postProcessingDialog->clusterAngle();
05380 int detectLoopClosureIterations = _postProcessingDialog->iterations();
05381 bool sba = _postProcessingDialog->isSBA();
05382 int sbaIterations = _postProcessingDialog->sbaIterations();
05383 double sbaVariance = _postProcessingDialog->sbaVariance();
05384 Optimizer::Type sbaType = _postProcessingDialog->sbaType();
05385
05386 if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
05387 {
05388 UWARN("No post-processing selection...");
05389 return;
05390 }
05391
05392
05393 bool allDataAvailable = true;
05394 std::map<int, Transform> odomPoses;
05395 for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
05396 iter!=_currentPosesMap.end() && allDataAvailable;
05397 ++iter)
05398 {
05399 QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
05400 if(jter != _cachedSignatures.end())
05401 {
05402 if(jter->getPose().isNull())
05403 {
05404 UWARN("Odometry pose of %d is null.", iter->first);
05405 allDataAvailable = false;
05406 }
05407 else
05408 {
05409 odomPoses.insert(*iter);
05410 }
05411 }
05412 else
05413 {
05414 UWARN("Node %d missing.", iter->first);
05415 allDataAvailable = false;
05416 }
05417 }
05418
05419 if(!allDataAvailable)
05420 {
05421 QMessageBox::warning(this, tr("Not all data available in the GUI..."),
05422 tr("Some data missing in the cache to respect the constraints chosen. "
05423 "Try \"Edit->Download all clouds\" to update the cache and try again."));
05424 return;
05425 }
05426
05427 _progressDialog->resetProgress();
05428 _progressDialog->clear();
05429 _progressDialog->show();
05430 _progressDialog->appendText("Post-processing beginning!");
05431 _progressDialog->setCancelButtonVisible(true);
05432 _progressCanceled = false;
05433
05434 int totalSteps = 0;
05435 if(refineNeighborLinks)
05436 {
05437 totalSteps+=(int)odomPoses.size();
05438 }
05439 if(refineLoopClosureLinks)
05440 {
05441 totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
05442 }
05443 if(sba)
05444 {
05445 totalSteps+=1;
05446 }
05447 _progressDialog->setMaximumSteps(totalSteps);
05448 _progressDialog->show();
05449
05450 ParametersMap parameters = _preferencesDialog->getAllParameters();
05451 Optimizer * optimizer = Optimizer::create(parameters);
05452 bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
05453 float optimizeMaxError = Parameters::defaultRGBDOptimizeMaxError();
05454 int optimizeIterations = Parameters::defaultOptimizerIterations();
05455 bool reextractFeatures = Parameters::defaultRGBDLoopClosureReextractFeatures();
05456 Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
05457 Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), optimizeMaxError);
05458 Parameters::parse(parameters, Parameters::kOptimizerIterations(), optimizeIterations);
05459 Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), reextractFeatures);
05460
05461 bool warn = false;
05462 int loopClosuresAdded = 0;
05463 std::multimap<int, int> checkedLoopClosures;
05464 if(detectMoreLoopClosures)
05465 {
05466 UDEBUG("");
05467
05468 UASSERT(detectLoopClosureIterations>0);
05469 for(int n=0; n<detectLoopClosureIterations && !_progressCanceled; ++n)
05470 {
05471 _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
05472 .arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
05473
05474 std::multimap<int, int> clusters = graph::radiusPosesClustering(
05475 _currentPosesMap,
05476 clusterRadius,
05477 clusterAngle*CV_PI/180.0);
05478
05479 _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+(int)clusters.size());
05480 _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
05481 QApplication::processEvents();
05482
05483 int i=0;
05484 std::set<int> addedLinks;
05485 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)
05486 {
05487 int from = iter->first;
05488 int to = iter->second;
05489 if(iter->first < iter->second)
05490 {
05491 from = iter->second;
05492 to = iter->first;
05493 }
05494
05495 bool alreadyChecked = false;
05496 for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
05497 !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
05498 ++jter)
05499 {
05500 if(to == jter->second)
05501 {
05502 alreadyChecked = true;
05503 }
05504 }
05505
05506 if(!alreadyChecked)
05507 {
05508
05509 if(addedLinks.find(from) == addedLinks.end() &&
05510 rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
05511 {
05512 checkedLoopClosures.insert(std::make_pair(from, to));
05513
05514 if(!_cachedSignatures.contains(from))
05515 {
05516 UERROR("Didn't find signature %d", from);
05517 }
05518 else if(!_cachedSignatures.contains(to))
05519 {
05520 UERROR("Didn't find signature %d", to);
05521 }
05522 else
05523 {
05524 Signature signatureFrom = _cachedSignatures[from];
05525 Signature signatureTo = _cachedSignatures[to];
05526
05527 if(signatureFrom.getWeight() >= 0 &&
05528 signatureTo.getWeight() >= 0)
05529 {
05530 Transform transform;
05531 RegistrationInfo info;
05532 if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
05533 parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
05534 {
05535 uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
05536 }
05537 Registration * registration = Registration::create(parameters);
05538
05539 if(reextractFeatures)
05540 {
05541 signatureFrom.sensorData().uncompressData();
05542 signatureTo.sensorData().uncompressData();
05543
05544 if(signatureFrom.sensorData().imageRaw().empty() &&
05545 signatureTo.sensorData().imageRaw().empty())
05546 {
05547 UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
05548 "images. Update the cache.",
05549 Parameters::kRGBDLoopClosureReextractFeatures().c_str());
05550 }
05551 else
05552 {
05553 signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
05554 signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
05555 signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
05556 signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
05557 signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
05558 signatureTo.setWords3(std::multimap<int, cv::Point3f>());
05559 signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
05560 signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
05561 }
05562 }
05563 else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
05564 {
05565 UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
05566 "registration will not be possible. Set \"%s\" to true.",
05567 Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
05568 signatureFrom.id(),
05569 signatureTo.id(),
05570 Parameters::kRGBDLoopClosureReextractFeatures().c_str());
05571 }
05572 transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
05573 delete registration;
05574 if(!transform.isNull())
05575 {
05576 if(!transform.isIdentity())
05577 {
05578
05579 info.covariance *= transform.getNorm();
05580 if(info.covariance.at<double>(0,0)<=0.0)
05581 {
05582 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001;
05583 }
05584 }
05585
05586
05587 bool updateConstraint = true;
05588 if(optimizeMaxError > 0.0f && optimizeIterations > 0)
05589 {
05590 int fromId = from;
05591 int mapId = _currentMapIds.at(from);
05592
05593 for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
05594 {
05595 if(iter->second == mapId && odomPoses.find(iter->first)!=odomPoses.end())
05596 {
05597 fromId = iter->first;
05598 break;
05599 }
05600 }
05601 std::multimap<int, Link> linksIn = _currentLinksMap;
05602 linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
05603 const Link * maxLinearLink = 0;
05604 const Link * maxAngularLink = 0;
05605 float maxLinearError = 0.0f;
05606 float maxAngularError = 0.0f;
05607 std::map<int, Transform> poses;
05608 std::multimap<int, Link> links;
05609 UASSERT(odomPoses.find(fromId) != odomPoses.end());
05610 UASSERT_MSG(odomPoses.find(from) != odomPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
05611 UASSERT_MSG(odomPoses.find(to) != odomPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
05612 optimizer->getConnectedGraph(fromId, odomPoses, linksIn, poses, links);
05613 UASSERT(poses.find(fromId) != poses.end());
05614 UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
05615 UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
05616 UASSERT(graph::findLink(links, from, to) != links.end());
05617 poses = optimizer->optimize(fromId, poses, links);
05618 std::string msg;
05619 if(poses.size())
05620 {
05621 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
05622 {
05623
05624 if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
05625 {
05626 UASSERT(poses.find(iter->second.from())!=poses.end());
05627 UASSERT(poses.find(iter->second.to())!=poses.end());
05628 Transform t1 = poses.at(iter->second.from());
05629 Transform t2 = poses.at(iter->second.to());
05630 UASSERT(!t1.isNull() && !t2.isNull());
05631 Transform t = t1.inverse()*t2;
05632 float linearError = uMax3(
05633 fabs(iter->second.transform().x() - t.x()),
05634 fabs(iter->second.transform().y() - t.y()),
05635 fabs(iter->second.transform().z() - t.z()));
05636 Eigen::Vector3f vA = t1.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
05637 Eigen::Vector3f vB = t2.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
05638 float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
05639 if(linearError > maxLinearError)
05640 {
05641 maxLinearError = linearError;
05642 maxLinearLink = &iter->second;
05643 }
05644 if(angularError > maxAngularError)
05645 {
05646 maxAngularError = angularError;
05647 maxAngularLink = &iter->second;
05648 }
05649 }
05650 }
05651 if(maxLinearLink)
05652 {
05653 UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
05654 }
05655 if(maxAngularLink)
05656 {
05657 UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
05658 }
05659
05660 if(maxLinearError > optimizeMaxError)
05661 {
05662 msg = uFormat("Rejecting edge %d->%d because "
05663 "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
05664 "\"%s\" is %f m.",
05665 from,
05666 to,
05667 maxLinearError,
05668 maxLinearLink->from(),
05669 maxLinearLink->to(),
05670 maxAngularError*180.0f/M_PI,
05671 maxAngularLink?maxAngularLink->from():0,
05672 maxAngularLink?maxAngularLink->to():0,
05673 Parameters::kRGBDOptimizeMaxError().c_str(),
05674 optimizeMaxError);
05675 }
05676 }
05677 else
05678 {
05679 msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
05680 from,
05681 to);
05682 }
05683 if(!msg.empty())
05684 {
05685 UWARN("%s", msg.c_str());
05686 _progressDialog->appendText(tr("%1").arg(msg.c_str()));
05687 QApplication::processEvents();
05688 updateConstraint = false;
05689 }
05690 }
05691
05692 if(updateConstraint)
05693 {
05694 UINFO("Added new loop closure between %d and %d.", from, to);
05695 addedLinks.insert(from);
05696 addedLinks.insert(to);
05697
05698 _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
05699 ++loopClosuresAdded;
05700 _progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
05701 }
05702 }
05703 }
05704 }
05705 }
05706 }
05707 QApplication::processEvents();
05708 _progressDialog->incrementStep();
05709 }
05710 _progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
05711 if(addedLinks.size() == 0)
05712 {
05713 break;
05714 }
05715
05716 if(n+1 < detectLoopClosureIterations)
05717 {
05718 _progressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
05719 .arg(odomPoses.size()).arg(_currentLinksMap.size()));
05720 QApplication::processEvents();
05721
05722 int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
05723 std::map<int, rtabmap::Transform> posesOut;
05724 std::multimap<int, rtabmap::Link> linksOut;
05725 std::map<int, rtabmap::Transform> optimizedPoses;
05726 optimizer->getConnectedGraph(
05727 fromId,
05728 odomPoses,
05729 _currentLinksMap,
05730 posesOut,
05731 linksOut);
05732 optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
05733 _currentPosesMap = optimizedPoses;
05734 _progressDialog->appendText(tr("Optimizing graph with new links... done!"));
05735 }
05736 }
05737 UINFO("Added %d loop closures.", loopClosuresAdded);
05738 _progressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
05739 }
05740
05741 if(!_progressCanceled && (refineNeighborLinks || refineLoopClosureLinks))
05742 {
05743 UDEBUG("");
05744 if(refineLoopClosureLinks)
05745 {
05746 _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+loopClosuresAdded);
05747 }
05748
05749 _progressDialog->appendText(tr("Refining links..."));
05750 QApplication::processEvents();
05751
05752 RegistrationIcp regIcp(parameters);
05753
05754 int i=0;
05755 for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end() && !_progressCanceled; ++iter, ++i)
05756 {
05757 int type = iter->second.type();
05758
05759 if((refineNeighborLinks && type==Link::kNeighbor) ||
05760 (refineLoopClosureLinks && type!=Link::kNeighbor))
05761 {
05762 int from = iter->second.from();
05763 int to = iter->second.to();
05764
05765 _progressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
05766 _progressDialog->incrementStep();
05767 QApplication::processEvents();
05768
05769 if(!_cachedSignatures.contains(from))
05770 {
05771 UERROR("Didn't find signature %d",from);
05772 }
05773 else if(!_cachedSignatures.contains(to))
05774 {
05775 UERROR("Didn't find signature %d", to);
05776 }
05777 else
05778 {
05779 Signature & signatureFrom = _cachedSignatures[from];
05780 Signature & signatureTo = _cachedSignatures[to];
05781
05782 LaserScan tmp;
05783 signatureFrom.sensorData().uncompressData(0,0,&tmp);
05784 signatureTo.sensorData().uncompressData(0,0,&tmp);
05785
05786 if(!signatureFrom.sensorData().laserScanRaw().isEmpty() &&
05787 !signatureTo.sensorData().laserScanRaw().isEmpty())
05788 {
05789 RegistrationInfo info;
05790 Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
05791
05792 if(!transform.isNull())
05793 {
05794 Link newLink(from, to, iter->second.type(), transform, info.covariance.inv());
05795 iter->second = newLink;
05796 }
05797 else
05798 {
05799 QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg.c_str());
05800 _progressDialog->appendText(str, Qt::darkYellow);
05801 UWARN("%s", str.toStdString().c_str());
05802 warn = true;
05803 }
05804 }
05805 else
05806 {
05807 QString str;
05808 if(signatureFrom.getWeight() < 0 || signatureTo.getWeight() < 0)
05809 {
05810 str = tr("Cannot refine link %1->%2 (Intermediate node detected!)").arg(from).arg(to);
05811 }
05812 else
05813 {
05814 str = tr("Cannot refine link %1->%2 (scans empty!)").arg(from).arg(to);
05815 }
05816
05817 _progressDialog->appendText(str, Qt::darkYellow);
05818 UWARN("%s", str.toStdString().c_str());
05819 warn = true;
05820 }
05821 }
05822 }
05823 }
05824 _progressDialog->appendText(tr("Refining links...done!"));
05825 }
05826
05827 _progressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
05828 .arg(odomPoses.size()).arg(_currentLinksMap.size()));
05829
05830 int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
05831 std::map<int, rtabmap::Transform> posesOut;
05832 std::multimap<int, rtabmap::Link> linksOut;
05833 std::map<int, rtabmap::Transform> optimizedPoses;
05834 optimizer->getConnectedGraph(
05835 fromId,
05836 odomPoses,
05837 _currentLinksMap,
05838 posesOut,
05839 linksOut);
05840 optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
05841 _progressDialog->appendText(tr("Optimizing graph with updated links... done!"));
05842 _progressDialog->incrementStep();
05843
05844 if(!_progressCanceled && sba)
05845 {
05846 UASSERT(Optimizer::isAvailable(sbaType));
05847 _progressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
05848 .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
05849 QApplication::processEvents();
05850 uSleep(100);
05851 QApplication::processEvents();
05852
05853 ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
05854 uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
05855 uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
05856 Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
05857 std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
05858 delete sba;
05859 if(newPoses.size())
05860 {
05861 optimizedPoses = newPoses;
05862 _progressDialog->appendText(tr("SBA... done!"));
05863 }
05864 else
05865 {
05866 _progressDialog->appendText(tr("SBA... failed!"));
05867 _progressDialog->setAutoClose(false);
05868 }
05869 _progressDialog->incrementStep();
05870 }
05871
05872 _progressDialog->appendText(tr("Updating map..."));
05873 this->updateMapCloud(
05874 optimizedPoses,
05875 std::multimap<int, Link>(_currentLinksMap),
05876 std::map<int, int>(_currentMapIds),
05877 std::map<int, std::string>(_currentLabels),
05878 std::map<int, Transform>(_currentGTPosesMap),
05879 false);
05880 _progressDialog->appendText(tr("Updating map... done!"));
05881
05882 if(warn)
05883 {
05884 _progressDialog->setAutoClose(false);
05885 }
05886
05887 _progressDialog->setValue(_progressDialog->maximumSteps());
05888 _progressDialog->appendText("Post-processing finished!");
05889 _progressDialog->setCancelButtonVisible(false);
05890 _progressCanceled = false;
05891
05892 delete optimizer;
05893 }
05894
05895 void MainWindow::depthCalibration()
05896 {
05897 if(_currentPosesMap.size() && _cachedSignatures.size())
05898 {
05899 _depthCalibrationDialog->calibrate(
05900 _currentPosesMap,
05901 _cachedSignatures,
05902 _preferencesDialog->getWorkingDirectory(),
05903 _preferencesDialog->getAllParameters());
05904 }
05905 else
05906 {
05907 QMessageBox::warning(this, tr("Depth Calibration"), tr("No data in cache. Try to refresh the cache."));
05908 }
05909 }
05910
05911 void MainWindow::deleteMemory()
05912 {
05913 QMessageBox::StandardButton button;
05914 if(_state == kMonitoring || _state == kMonitoringPaused)
05915 {
05916 button = QMessageBox::question(this,
05917 tr("Deleting memory..."),
05918 tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
05919 QMessageBox::Yes|QMessageBox::No,
05920 QMessageBox::No);
05921 }
05922 else
05923 {
05924 button = QMessageBox::question(this,
05925 tr("Deleting memory..."),
05926 tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
05927 QMessageBox::Yes|QMessageBox::No,
05928 QMessageBox::No);
05929 }
05930
05931 if(button != QMessageBox::Yes)
05932 {
05933 return;
05934 }
05935
05936 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
05937 if(_state!=kDetecting)
05938 {
05939 _databaseUpdated = false;
05940 }
05941 this->clearTheCache();
05942 }
05943
05944 QString MainWindow::getWorkingDirectory() const
05945 {
05946 return _preferencesDialog->getWorkingDirectory();
05947 }
05948
05949 void MainWindow::openWorkingDirectory()
05950 {
05951 QString filePath = _preferencesDialog->getWorkingDirectory();
05952 #if defined(Q_WS_MAC)
05953 QStringList args;
05954 args << "-e";
05955 args << "tell application \"Finder\"";
05956 args << "-e";
05957 args << "activate";
05958 args << "-e";
05959 args << "select POSIX file \""+filePath+"\"";
05960 args << "-e";
05961 args << "end tell";
05962 QProcess::startDetached("osascript", args);
05963 #elif defined(Q_WS_WIN)
05964 QStringList args;
05965 args << "/select," << QDir::toNativeSeparators(filePath);
05966 QProcess::startDetached("explorer", args);
05967 #else
05968 UERROR("Only works on Mac and Windows");
05969 #endif
05970 }
05971
05972 void MainWindow::updateEditMenu()
05973 {
05974
05975 if(_state != kMonitoring && _state != kMonitoringPaused && (!_openedDatabasePath.isEmpty() || !_newDatabasePath.isEmpty()))
05976 {
05977 if(!_openedDatabasePath.isEmpty())
05978 {
05979 _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_openedDatabasePath.toStdString())/1000000));
05980 }
05981 else
05982 {
05983 _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_newDatabasePath.toStdString())/1000000));
05984 }
05985 }
05986 }
05987
05988 void MainWindow::selectStream()
05989 {
05990 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice);
05991 }
05992
05993 void MainWindow::selectOpenni()
05994 {
05995 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
05996 }
05997
05998 void MainWindow::selectFreenect()
05999 {
06000 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
06001 }
06002
06003 void MainWindow::selectOpenniCv()
06004 {
06005 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
06006 }
06007
06008 void MainWindow::selectOpenniCvAsus()
06009 {
06010 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
06011 }
06012
06013 void MainWindow::selectOpenni2()
06014 {
06015 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
06016 }
06017
06018 void MainWindow::selectFreenect2()
06019 {
06020 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
06021 }
06022
06023 void MainWindow::selectK4W2()
06024 {
06025 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
06026 }
06027
06028 void MainWindow::selectRealSense()
06029 {
06030 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);
06031 }
06032
06033 void MainWindow::selectRealSense2()
06034 {
06035 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2);
06036 }
06037
06038 void MainWindow::selectStereoDC1394()
06039 {
06040 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
06041 }
06042
06043 void MainWindow::selectStereoFlyCapture2()
06044 {
06045 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
06046 }
06047 void MainWindow::selectStereoZed()
06048 {
06049 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed);
06050 }
06051
06052 void MainWindow::selectStereoUsb()
06053 {
06054 _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
06055 }
06056
06057 void MainWindow::dumpTheMemory()
06058 {
06059 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
06060 }
06061
06062 void MainWindow::dumpThePrediction()
06063 {
06064 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
06065 }
06066
06067 void MainWindow::sendGoal()
06068 {
06069 UINFO("Sending a goal...");
06070 bool ok = false;
06071 QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
06072 if(ok && !text.isEmpty())
06073 {
06074 _waypoints.clear();
06075 _waypointsIndex = 0;
06076
06077 this->postGoal(text);
06078 }
06079 }
06080
06081 void MainWindow::sendWaypoints()
06082 {
06083 UINFO("Sending waypoints...");
06084 bool ok = false;
06085 QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
06086 if(ok && !text.isEmpty())
06087 {
06088 QStringList wp = text.split(' ');
06089 if(wp.size() < 2)
06090 {
06091 QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
06092 }
06093 else
06094 {
06095 _waypoints = wp;
06096 _waypointsIndex = 0;
06097 this->postGoal(_waypoints.at(_waypointsIndex));
06098 }
06099 }
06100 }
06101
06102 void MainWindow::postGoal(const QString & goal)
06103 {
06104 if(!goal.isEmpty())
06105 {
06106 bool ok = false;
06107 int id = goal.toInt(&ok);
06108 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
06109 UINFO("Posting event with goal %s", goal.toStdString().c_str());
06110 if(ok)
06111 {
06112 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
06113 }
06114 else
06115 {
06116 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
06117 }
06118 }
06119 }
06120
06121 void MainWindow::cancelGoal()
06122 {
06123 UINFO("Cancelling goal...");
06124 _waypoints.clear();
06125 _waypointsIndex = 0;
06126 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
06127 }
06128
06129 void MainWindow::label()
06130 {
06131 UINFO("Labelling current location...");
06132 bool ok = false;
06133 QString label = QInputDialog::getText(this, tr("Label current location"), tr("Label: "), QLineEdit::Normal, "", &ok);
06134 if(ok && !label.isEmpty())
06135 {
06136 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdLabel, label.toStdString(), 0));
06137 }
06138 }
06139
06140 void MainWindow::updateCacheFromDatabase()
06141 {
06142 QString dir = getWorkingDirectory();
06143 QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
06144 if(!path.isEmpty())
06145 {
06146 updateCacheFromDatabase(path);
06147 }
06148 }
06149
06150 void MainWindow::updateCacheFromDatabase(const QString & path)
06151 {
06152 if(!path.isEmpty())
06153 {
06154 DBDriver * driver = DBDriver::create();
06155 if(driver->openConnection(path.toStdString()))
06156 {
06157 UINFO("Update cache...");
06158 _progressDialog->resetProgress();
06159 _progressDialog->show();
06160 _progressDialog->appendText(tr("Downloading the map from \"%1\" (without poses and links)...")
06161 .arg(path));
06162
06163 std::set<int> ids;
06164 driver->getAllNodeIds(ids, true);
06165 std::list<Signature*> signaturesList;
06166 driver->loadSignatures(std::list<int>(ids.begin(), ids.end()), signaturesList);
06167 std::map<int, Signature> signatures;
06168 driver->loadNodeData(signaturesList);
06169 for(std::list<Signature *>::iterator iter=signaturesList.begin(); iter!=signaturesList.end(); ++iter)
06170 {
06171 signatures.insert(std::make_pair((*iter)->id(), *(*iter)));
06172 delete *iter;
06173 }
06174 RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap);
06175 processRtabmapEvent3DMap(event);
06176 }
06177 else
06178 {
06179 QMessageBox::warning(this, tr("Update cache"), tr("Failed to open database \"%1\"").arg(path));
06180 }
06181 delete driver;
06182 }
06183 }
06184
06185 void MainWindow::downloadAllClouds()
06186 {
06187 QStringList items;
06188 items.append("Local map optimized");
06189 items.append("Local map not optimized");
06190 items.append("Global map optimized");
06191 items.append("Global map not optimized");
06192
06193 bool ok;
06194 QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 2, false, &ok);
06195 if(ok)
06196 {
06197 bool optimized=false, global=false;
06198 if(item.compare("Local map optimized") == 0)
06199 {
06200 optimized = true;
06201 }
06202 else if(item.compare("Local map not optimized") == 0)
06203 {
06204
06205 }
06206 else if(item.compare("Global map optimized") == 0)
06207 {
06208 global=true;
06209 optimized=true;
06210 }
06211 else if(item.compare("Global map not optimized") == 0)
06212 {
06213 global=true;
06214 }
06215 else
06216 {
06217 UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
06218 }
06219
06220 UINFO("Download clouds...");
06221 _progressDialog->resetProgress();
06222 _progressDialog->show();
06223 _progressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
06224 .arg(global?"true":"false").arg(optimized?"true":"false"));
06225 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
06226 }
06227 }
06228
06229 void MainWindow::downloadPoseGraph()
06230 {
06231 QStringList items;
06232 items.append("Local map optimized");
06233 items.append("Local map not optimized");
06234 items.append("Global map optimized");
06235 items.append("Global map not optimized");
06236
06237 bool ok;
06238 QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 2, false, &ok);
06239 if(ok)
06240 {
06241 bool optimized=false, global=false;
06242 if(item.compare("Local map optimized") == 0)
06243 {
06244 optimized = true;
06245 }
06246 else if(item.compare("Local map not optimized") == 0)
06247 {
06248
06249 }
06250 else if(item.compare("Global map optimized") == 0)
06251 {
06252 global=true;
06253 optimized=true;
06254 }
06255 else if(item.compare("Global map not optimized") == 0)
06256 {
06257 global=true;
06258 }
06259 else
06260 {
06261 UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
06262 }
06263
06264 UINFO("Download the graph...");
06265 _progressDialog->resetProgress();
06266 _progressDialog->show();
06267 _progressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
06268 .arg(global?"true":"false").arg(optimized?"true":"false"));
06269
06270 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
06271 }
06272 }
06273
06274 void MainWindow::anchorCloudsToGroundTruth()
06275 {
06276 this->updateMapCloud(
06277 std::map<int, Transform>(_currentPosesMap),
06278 std::multimap<int, Link>(_currentLinksMap),
06279 std::map<int, int>(_currentMapIds),
06280 std::map<int, std::string>(_currentLabels),
06281 std::map<int, Transform>(_currentGTPosesMap));
06282 }
06283
06284 void MainWindow::clearTheCache()
06285 {
06286 _cachedSignatures.clear();
06287 _cachedMemoryUsage = 0;
06288 _cachedWordsCount.clear();
06289 _cachedClouds.clear();
06290 _createdCloudsMemoryUsage = 0;
06291 _cachedEmptyClouds.clear();
06292 _previousCloud.first = 0;
06293 _previousCloud.second.first.first.reset();
06294 _previousCloud.second.first.second.reset();
06295 _previousCloud.second.second.reset();
06296 _createdScans.clear();
06297 _createdFeatures.clear();
06298 _cloudViewer->clear();
06299 _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
06300 _cloudViewer->clearTrajectory();
06301 _ui->widget_mapVisibility->clear();
06302 _currentPosesMap.clear();
06303 _currentGTPosesMap.clear();
06304 _currentLinksMap.clear();
06305 _currentMapIds.clear();
06306 _currentLabels.clear();
06307 _odometryCorrection = Transform::getIdentity();
06308 _lastOdomPose.setNull();
06309 _ui->statsToolBox->clear();
06310
06311 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
06312 _ui->actionPost_processing->setEnabled(false);
06313 _ui->actionSave_point_cloud->setEnabled(false);
06314 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
06315 _ui->actionDepth_Calibration->setEnabled(false);
06316 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
06317 _ui->actionExport_octomap->setEnabled(false);
06318 _ui->actionView_high_res_point_cloud->setEnabled(false);
06319 _likelihoodCurve->clear();
06320 _rawLikelihoodCurve->clear();
06321 _posteriorCurve->clear();
06322 _lastId = 0;
06323 _lastIds.clear();
06324 _firstStamp = 0.0f;
06325 _ui->label_stats_loopClosuresDetected->setText("0");
06326 _ui->label_stats_loopClosuresReactivatedDetected->setText("0");
06327 _ui->label_stats_loopClosuresRejected->setText("0");
06328 _refIds.clear();
06329 _loopClosureIds.clear();
06330 _cachedLocalizationsCount.clear();
06331 _ui->label_refId->clear();
06332 _ui->label_matchId->clear();
06333 _ui->graphicsView_graphView->clearAll();
06334 _ui->imageView_source->clear();
06335 _ui->imageView_loopClosure->clear();
06336 _ui->imageView_odometry->clear();
06337 _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
06338 _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
06339 _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
06340 #ifdef RTABMAP_OCTOMAP
06341
06342 UASSERT(_octomap != 0);
06343 delete _octomap;
06344 _octomap = new OctoMap(_preferencesDialog->getAllParameters());
06345 #endif
06346 _occupancyGrid->clear();
06347 _rectCameraModels.clear();
06348 }
06349
06350 void MainWindow::openHelp()
06351 {
06352 if(_state == kMonitoringPaused || _state == kMonitoring)
06353 {
06354 QDesktopServices::openUrl(QUrl("http://wiki.ros.org/rtabmap_ros"));
06355 }
06356 else
06357 {
06358 QDesktopServices::openUrl(QUrl("https://github.com/introlab/rtabmap/wiki"));
06359 }
06360 }
06361
06362 void MainWindow::updateElapsedTime()
06363 {
06364 if(_state == kDetecting || _state == kMonitoring)
06365 {
06366 QString format = "hh:mm:ss";
06367 _ui->label_elapsedTime->setText((QTime().fromString(_ui->label_elapsedTime->text(), format).addMSecs(_elapsedTime->restart())).toString(format));
06368 }
06369 }
06370
06371 void MainWindow::saveFigures()
06372 {
06373 QList<int> curvesPerFigure;
06374 QStringList curveNames;
06375 _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
06376
06377 QStringList curvesPerFigureStr;
06378 for(int i=0; i<curvesPerFigure.size(); ++i)
06379 {
06380 curvesPerFigureStr.append(QString::number(curvesPerFigure[i]));
06381 }
06382 for(int i=0; i<curveNames.size(); ++i)
06383 {
06384 curveNames[i].replace(' ', '_');
06385 }
06386 _preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
06387 _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
06388 }
06389
06390 void MainWindow::loadFigures()
06391 {
06392 QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
06393 QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
06394
06395 if(!curvesPerFigure.isEmpty())
06396 {
06397 QStringList curvesPerFigureList = curvesPerFigure.split(" ");
06398 QStringList curvesNamesList = curveNames.split(" ");
06399
06400 int j=0;
06401 for(int i=0; i<curvesPerFigureList.size(); ++i)
06402 {
06403 bool ok = false;
06404 int count = curvesPerFigureList[i].toInt(&ok);
06405 if(!ok)
06406 {
06407 QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
06408 break;
06409 }
06410 else
06411 {
06412 _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
06413 for(int k=1; k<count && j<curveNames.size(); ++k)
06414 {
06415 _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
06416 }
06417 }
06418 }
06419 }
06420
06421 }
06422
06423 void MainWindow::openPreferences()
06424 {
06425 _preferencesDialog->setMonitoringState(_state == kMonitoring || _state == kMonitoringPaused);
06426 _preferencesDialog->exec();
06427 }
06428
06429 void MainWindow::openPreferencesSource()
06430 {
06431 _preferencesDialog->setCurrentPanelToSource();
06432 openPreferences();
06433 this->updateSelectSourceMenu();
06434 }
06435
06436 void MainWindow::setDefaultViews()
06437 {
06438 _ui->dockWidget_posterior->setVisible(false);
06439 _ui->dockWidget_likelihood->setVisible(false);
06440 _ui->dockWidget_rawlikelihood->setVisible(false);
06441 _ui->dockWidget_statsV2->setVisible(false);
06442 _ui->dockWidget_console->setVisible(false);
06443 _ui->dockWidget_loopClosureViewer->setVisible(false);
06444 _ui->dockWidget_mapVisibility->setVisible(false);
06445 _ui->dockWidget_graphViewer->setVisible(false);
06446 _ui->dockWidget_odometry->setVisible(true);
06447 _ui->dockWidget_cloudViewer->setVisible(true);
06448 _ui->dockWidget_imageView->setVisible(true);
06449 _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused);
06450 _ui->toolBar_2->setVisible(true);
06451 _ui->statusbar->setVisible(false);
06452 this->setAspectRatio720p();
06453 _cloudViewer->resetCamera();
06454 _cloudViewer->setCameraLockZ(true);
06455 _cloudViewer->setCameraTargetFollow(true);
06456 }
06457
06458 void MainWindow::selectScreenCaptureFormat(bool checked)
06459 {
06460 if(checked)
06461 {
06462 QStringList items;
06463 items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
06464 bool ok;
06465 QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
06466 if(ok && !item.isEmpty())
06467 {
06468 if(item.compare("Synchronize with map update") == 0)
06469 {
06470 _autoScreenCaptureOdomSync = false;
06471 }
06472 else
06473 {
06474 _autoScreenCaptureOdomSync = true;
06475 }
06476
06477 if(_state != kMonitoring && _state != kMonitoringPaused)
06478 {
06479 int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM? Images will be saved on disk when clicking auto screen capture again."), QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
06480 if(r == QMessageBox::No || r == QMessageBox::Yes)
06481 {
06482 _autoScreenCaptureRAM = r == QMessageBox::Yes;
06483 }
06484 else
06485 {
06486 _ui->actionAuto_screen_capture->setChecked(false);
06487 }
06488
06489 r = QMessageBox::question(this, tr("Save in JPEG?"), tr("Save in JPEG format? Otherwise they are saved in PNG."), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
06490 if(r == QMessageBox::No || r == QMessageBox::Yes)
06491 {
06492 _autoScreenCapturePNG = r == QMessageBox::No;
06493 }
06494 else
06495 {
06496 _ui->actionAuto_screen_capture->setChecked(false);
06497 }
06498 }
06499 }
06500 else
06501 {
06502 _ui->actionAuto_screen_capture->setChecked(false);
06503 }
06504 }
06505 else if(_autoScreenCaptureCachedImages.size())
06506 {
06507 QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
06508 QDir dir;
06509 if(!dir.exists(targetDir))
06510 {
06511 dir.mkdir(targetDir);
06512 }
06513 targetDir += QDir::separator();
06514 targetDir += "Main_window";
06515 if(!dir.exists(targetDir))
06516 {
06517 dir.mkdir(targetDir);
06518 }
06519 targetDir += QDir::separator();
06520
06521 _progressDialog->setCancelButtonVisible(true);
06522 _progressDialog->resetProgress();
06523 _progressDialog->show();
06524 _progressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size());
06525 int i=0;
06526 for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end() && !_progressDialog->isCanceled(); ++iter)
06527 {
06528 QPixmap figure;
06529 figure.loadFromData(iter.value(), _autoScreenCapturePNG?"PNG":"JPEG");
06530 figure.save(targetDir + iter.key(), _autoScreenCapturePNG?"PNG":"JPEG");
06531 _progressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size()));
06532 _progressDialog->incrementStep();
06533 QApplication::processEvents();
06534 }
06535 _autoScreenCaptureCachedImages.clear();
06536 _progressDialog->setValue(_progressDialog->maximumSteps());
06537 _progressDialog->setCancelButtonVisible(false);
06538 }
06539 }
06540
06541 void MainWindow::takeScreenshot()
06542 {
06543 QDesktopServices::openUrl(QUrl::fromLocalFile(this->captureScreen()));
06544 }
06545
06546 void MainWindow::setAspectRatio(int w, int h)
06547 {
06548 QRect rect = this->geometry();
06549 if(h<100 && w<100)
06550 {
06551
06552 if(float(rect.width())/float(rect.height()) > float(w)/float(h))
06553 {
06554 rect.setWidth(w*(rect.height()/h));
06555 rect.setHeight((rect.height()/h)*h);
06556 }
06557 else
06558 {
06559 rect.setHeight(h*(rect.width()/w));
06560 rect.setWidth((rect.width()/w)*w);
06561 }
06562 }
06563 else
06564 {
06565
06566 rect.setWidth(w);
06567 rect.setHeight(h);
06568 }
06569 this->setGeometry(rect);
06570 }
06571
06572 void MainWindow::setAspectRatio16_9()
06573 {
06574 this->setAspectRatio(16, 9);
06575 }
06576
06577 void MainWindow::setAspectRatio16_10()
06578 {
06579 this->setAspectRatio(16, 10);
06580 }
06581
06582 void MainWindow::setAspectRatio4_3()
06583 {
06584 this->setAspectRatio(4, 3);
06585 }
06586
06587 void MainWindow::setAspectRatio240p()
06588 {
06589 this->setAspectRatio((240*16)/9, 240);
06590 }
06591
06592 void MainWindow::setAspectRatio360p()
06593 {
06594 this->setAspectRatio((360*16)/9, 360);
06595 }
06596
06597 void MainWindow::setAspectRatio480p()
06598 {
06599 this->setAspectRatio((480*16)/9, 480);
06600 }
06601
06602 void MainWindow::setAspectRatio720p()
06603 {
06604 this->setAspectRatio((720*16)/9, 720);
06605 }
06606
06607 void MainWindow::setAspectRatio1080p()
06608 {
06609 this->setAspectRatio((1080*16)/9, 1080);
06610 }
06611
06612 void MainWindow::setAspectRatioCustom()
06613 {
06614 bool ok;
06615 int width = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Width (pixels):"), this->geometry().width(), 100, 10000, 100, &ok);
06616 if(ok)
06617 {
06618 int height = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Height (pixels):"), this->geometry().height(), 100, 10000, 100, &ok);
06619 if(ok)
06620 {
06621 this->setAspectRatio(width, height);
06622 }
06623 }
06624 }
06625
06626 void MainWindow::exportGridMap()
06627 {
06628 float gridCellSize = 0.05f;
06629 bool ok;
06630 gridCellSize = (float)QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), (double)gridCellSize, 0.01, 1, 2, &ok);
06631 if(!ok)
06632 {
06633 return;
06634 }
06635
06636
06637 float xMin=0.0f, yMin=0.0f;
06638 cv::Mat pixels;
06639 #ifdef RTABMAP_OCTOMAP
06640 if(_preferencesDialog->isOctomap2dGrid())
06641 {
06642 pixels = _octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
06643
06644 }
06645 else
06646 #endif
06647 {
06648 pixels = _occupancyGrid->getMap(xMin, yMin);
06649 }
06650
06651 if(!pixels.empty())
06652 {
06653 cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
06654
06655 for (int i = 0; i < pixels.rows; ++i)
06656 {
06657 for (int j = 0; j < pixels.cols; ++j)
06658 {
06659 char v = pixels.at<char>(i, j);
06660 unsigned char gray;
06661 if(v == 0)
06662 {
06663 gray = 178;
06664 }
06665 else if(v == 100)
06666 {
06667 gray = 0;
06668 }
06669 else
06670 {
06671 gray = 89;
06672 }
06673 map8U.at<unsigned char>(i, j) = gray;
06674 }
06675 }
06676
06677 QImage image = uCvMat2QImage(map8U, false);
06678
06679 QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
06680 if(!path.isEmpty())
06681 {
06682 if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
06683 {
06684
06685 path += ".png";
06686 }
06687
06688 QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
06689 QPixmap::fromImage(img).save(path);
06690
06691 QDesktopServices::openUrl(QUrl::fromLocalFile(path));
06692 }
06693 }
06694 }
06695
06696 void MainWindow::exportClouds()
06697 {
06698 if(_exportCloudsDialog->isVisible())
06699 {
06700 return;
06701 }
06702
06703 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
06704
06705
06706 if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
06707 {
06708 for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
06709 {
06710 std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
06711 if(gtIter!=_currentGTPosesMap.end())
06712 {
06713 iter->second = gtIter->second;
06714 }
06715 else
06716 {
06717 UWARN("Not found ground truth pose for node %d", iter->first);
06718 }
06719 }
06720 }
06721
06722 _exportCloudsDialog->exportClouds(
06723 poses,
06724 _currentLinksMap,
06725 _currentMapIds,
06726 _cachedSignatures,
06727 _cachedClouds,
06728 _createdScans,
06729 _preferencesDialog->getWorkingDirectory(),
06730 _preferencesDialog->getAllParameters());
06731 }
06732
06733 void MainWindow::viewClouds()
06734 {
06735 if(_exportCloudsDialog->isVisible())
06736 {
06737 return;
06738 }
06739
06740 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
06741
06742
06743 if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
06744 {
06745 for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
06746 {
06747 std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
06748 if(gtIter!=_currentGTPosesMap.end())
06749 {
06750 iter->second = gtIter->second;
06751 }
06752 else
06753 {
06754 UWARN("Not found ground truth pose for node %d", iter->first);
06755 }
06756 }
06757 }
06758
06759 _exportCloudsDialog->viewClouds(
06760 poses,
06761 _currentLinksMap,
06762 _currentMapIds,
06763 _cachedSignatures,
06764 _cachedClouds,
06765 _createdScans,
06766 _preferencesDialog->getWorkingDirectory(),
06767 _preferencesDialog->getAllParameters());
06768
06769 }
06770
06771 void MainWindow::exportOctomap()
06772 {
06773 #ifdef RTABMAP_OCTOMAP
06774 if(_octomap->octree()->size())
06775 {
06776 QString path = QFileDialog::getSaveFileName(
06777 this,
06778 tr("Save File"),
06779 this->getWorkingDirectory()+"/"+"octomap.bt",
06780 tr("Octomap file (*.bt)"));
06781
06782 if(!path.isEmpty())
06783 {
06784 if(_octomap->writeBinary(path.toStdString()))
06785 {
06786 QMessageBox::information(this,
06787 tr("Export octomap..."),
06788 tr("Octomap successfully saved to \"%1\".")
06789 .arg(path));
06790 }
06791 else
06792 {
06793 QMessageBox::information(this,
06794 tr("Export octomap..."),
06795 tr("Failed to save octomap to \"%1\"!")
06796 .arg(path));
06797 }
06798 }
06799 }
06800 else
06801 {
06802 UERROR("Empty octomap.");
06803 }
06804 #else
06805 UERROR("Cannot export octomap, RTAB-Map is not built with it.");
06806 #endif
06807 }
06808
06809 void MainWindow::exportImages()
06810 {
06811 if(_cachedSignatures.empty())
06812 {
06813 QMessageBox::warning(this, tr("Export images..."), tr("Cannot export images, the cache is empty!"));
06814 return;
06815 }
06816 std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
06817
06818 if(poses.empty())
06819 {
06820 QMessageBox::warning(this, tr("Export images..."), tr("There is no map!"));
06821 return;
06822 }
06823
06824 QStringList formats;
06825 formats.push_back("jpg");
06826 formats.push_back("png");
06827 bool ok;
06828 QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
06829 if(!ok)
06830 {
06831 return;
06832 }
06833
06834 QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), this->getWorkingDirectory());
06835 if(!path.isEmpty())
06836 {
06837 SensorData data;
06838 if(_cachedSignatures.contains(poses.rbegin()->first))
06839 {
06840 data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
06841 data.uncompressData();
06842 }
06843 if(!data.imageRaw().empty() && !data.rightRaw().empty())
06844 {
06845 QDir dir;
06846 dir.mkdir(QString("%1/left").arg(path));
06847 dir.mkdir(QString("%1/right").arg(path));
06848 if(data.stereoCameraModel().isValidForProjection())
06849 {
06850 std::string cameraName = "calibration";
06851 StereoCameraModel model(
06852 cameraName,
06853 data.imageRaw().size(),
06854 data.stereoCameraModel().left().K(),
06855 data.stereoCameraModel().left().D(),
06856 data.stereoCameraModel().left().R(),
06857 data.stereoCameraModel().left().P(),
06858 data.rightRaw().size(),
06859 data.stereoCameraModel().right().K(),
06860 data.stereoCameraModel().right().D(),
06861 data.stereoCameraModel().right().R(),
06862 data.stereoCameraModel().right().P(),
06863 data.stereoCameraModel().R(),
06864 data.stereoCameraModel().T(),
06865 data.stereoCameraModel().E(),
06866 data.stereoCameraModel().F(),
06867 data.stereoCameraModel().left().localTransform());
06868 if(model.save(path.toStdString()))
06869 {
06870 UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
06871 }
06872 else
06873 {
06874 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
06875 }
06876 }
06877 }
06878 else if(!data.imageRaw().empty())
06879 {
06880 if(!data.depthRaw().empty())
06881 {
06882 QDir dir;
06883 dir.mkdir(QString("%1/rgb").arg(path));
06884 dir.mkdir(QString("%1/depth").arg(path));
06885 }
06886
06887 if(data.cameraModels().size() > 1)
06888 {
06889 UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
06890 }
06891 else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
06892 {
06893 std::string cameraName = "calibration";
06894 CameraModel model(cameraName,
06895 data.imageRaw().size(),
06896 data.cameraModels().front().K(),
06897 data.cameraModels().front().D(),
06898 data.cameraModels().front().R(),
06899 data.cameraModels().front().P(),
06900 data.cameraModels().front().localTransform());
06901 if(model.save(path.toStdString()))
06902 {
06903 UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
06904 }
06905 else
06906 {
06907 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
06908 }
06909 }
06910 }
06911 else
06912 {
06913 QMessageBox::warning(this,
06914 tr("Export images..."),
06915 tr("Data in the cache don't seem to have images (tested node %1). Calibration file will not be saved. Try refreshing the cache (with clouds).").arg(poses.rbegin()->first));
06916 }
06917
06918 _progressDialog->resetProgress();
06919 _progressDialog->show();
06920 _progressDialog->setMaximumSteps(_cachedSignatures.size());
06921
06922 unsigned int saved = 0;
06923 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
06924 {
06925 int id = iter->first;
06926 SensorData data;
06927 if(_cachedSignatures.contains(iter->first))
06928 {
06929 data = _cachedSignatures.value(iter->first).sensorData();
06930 data.uncompressData();
06931 }
06932 QString info;
06933 bool warn = false;
06934 if(!data.imageRaw().empty() && !data.rightRaw().empty())
06935 {
06936 cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
06937 cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
06938 info = tr("Saved left/%1.%2 and right/%1.%2.").arg(id).arg(ext);
06939 }
06940 else if(!data.imageRaw().empty() && !data.depthRaw().empty())
06941 {
06942 cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
06943 cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
06944 info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
06945 }
06946 else if(!data.imageRaw().empty())
06947 {
06948 cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
06949 info = tr("Saved %1.%2.").arg(id).arg(ext);
06950 }
06951 else
06952 {
06953 info = tr("No images saved for node %1!").arg(id);
06954 warn = true;
06955 }
06956 saved += warn?0:1;
06957 _progressDialog->appendText(info, !warn?Qt::black:Qt::darkYellow);
06958 _progressDialog->incrementStep();
06959 QApplication::processEvents();
06960
06961 }
06962 if(saved!=poses.size())
06963 {
06964 _progressDialog->setAutoClose(false);
06965 _progressDialog->appendText(tr("%1 images of %2 saved to \"%3\".").arg(saved).arg(poses.size()).arg(path));
06966 }
06967 else
06968 {
06969 _progressDialog->appendText(tr("%1 images saved to \"%2\".").arg(saved).arg(path));
06970 }
06971
06972 _progressDialog->setValue(_progressDialog->maximumSteps());
06973 }
06974 }
06975
06976 void MainWindow::exportBundlerFormat()
06977 {
06978 if(_exportBundlerDialog->isVisible())
06979 {
06980 return;
06981 }
06982
06983 std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
06984
06985
06986 if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
06987 {
06988 for(std::map<int, Transform>::iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
06989 {
06990 std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
06991 if(gtIter!=_currentGTPosesMap.end())
06992 {
06993 iter->second = gtIter->second;
06994 }
06995 else
06996 {
06997 UWARN("Not found ground truth pose for node %d", iter->first);
06998 }
06999 }
07000 }
07001
07002 std::map<int, Transform> poses;
07003 for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
07004 {
07005 if(_cachedSignatures.contains(iter->first))
07006 {
07007 if(_cachedSignatures[iter->first].sensorData().imageRaw().empty() &&
07008 _cachedSignatures[iter->first].sensorData().imageCompressed().empty())
07009 {
07010 UWARN("Missing image in cache for node %d", iter->first);
07011 }
07012 else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
07013 _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection())
07014 {
07015 poses.insert(*iter);
07016 }
07017 else
07018 {
07019 UWARN("Missing calibration for node %d", iter->first);
07020 }
07021 }
07022 else
07023 {
07024 UWARN("Did not find node %d in cache", iter->first);
07025 }
07026 }
07027
07028 if(poses.size())
07029 {
07030 if(_exportBundlerDialog->exec() != QDialog::Accepted)
07031 {
07032 return;
07033 }
07034 QString path = _exportBundlerDialog->outputPath();
07035 if(!path.isEmpty())
07036 {
07037 if(!QDir(path).mkpath("."))
07038 {
07039 QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
07040 return;
07041 }
07042
07043 QFile fileOut(path+QDir::separator()+"cameras.out");
07044 QFile fileList(path+QDir::separator()+"list.txt");
07045 QDir(path).mkdir("images");
07046 if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
07047 {
07048 if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
07049 {
07050 std::set<int> ignoredCameras;
07051 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
07052 {
07053 QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
07054 p = path+QDir::separator()+p;
07055 cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw();
07056 if(image.empty())
07057 {
07058 _cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
07059 }
07060
07061 double maxLinearVel = _exportBundlerDialog->maxLinearSpeed();
07062 double maxAngularVel = _exportBundlerDialog->maxAngularSpeed();
07063 double laplacianThr = _exportBundlerDialog->laplacianThreshold();
07064 bool blurryImage = false;
07065 const std::vector<float> & velocity = _cachedSignatures[iter->first].getVelocity();
07066 if(maxLinearVel>0.0 || maxAngularVel>0.0)
07067 {
07068 if(velocity.size() == 6)
07069 {
07070 float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
07071 float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
07072 if(maxLinearVel>0.0 && transVel > maxLinearVel)
07073 {
07074 UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
07075 blurryImage = true;
07076 }
07077 else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
07078 {
07079 UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
07080 blurryImage = true;
07081 }
07082 }
07083 else
07084 {
07085 UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
07086 }
07087 }
07088
07089 if(!blurryImage && !image.empty() && laplacianThr>0.0)
07090 {
07091 cv::Mat imgLaplacian;
07092 cv::Laplacian(image, imgLaplacian, CV_16S);
07093 cv::Mat m, s;
07094 cv::meanStdDev(imgLaplacian, m, s);
07095 double stddev_pxl = s.at<double>(0);
07096 double var = stddev_pxl*stddev_pxl;
07097 if(var < laplacianThr)
07098 {
07099 blurryImage = true;
07100 UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
07101 }
07102 }
07103 if(blurryImage)
07104 {
07105 ignoredCameras.insert(iter->first);
07106 }
07107 else
07108 {
07109 if(cv::imwrite(p.toStdString(), image))
07110 {
07111 UINFO("saved image %s", p.toStdString().c_str());
07112 }
07113 else
07114 {
07115 UERROR("Failed to save image %s", p.toStdString().c_str());
07116 }
07117 }
07118 }
07119
07120 QTextStream out(&fileOut);
07121 QTextStream list(&fileList);
07122 out << "# Bundle file v0.3\n";
07123 out << poses.size()-ignoredCameras.size() << " 0\n";
07124
07125 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
07126 {
07127 if(ignoredCameras.find(iter->first) == ignoredCameras.end())
07128 {
07129 QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
07130 list << p << "\n";
07131
07132 Transform localTransform;
07133 if(_cachedSignatures[iter->first].sensorData().cameraModels().size())
07134 {
07135 out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
07136 localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
07137 }
07138 else
07139 {
07140 out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
07141 localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
07142 }
07143
07144 static const Transform opengl_world_T_rtabmap_world(
07145 0.0f, -1.0f, 0.0f, 0.0f,
07146 0.0f, 0.0f, 1.0f, 0.0f,
07147 -1.0f, 0.0f, 0.0f, 0.0f);
07148
07149 static const Transform optical_rotation_inv(
07150 0.0f, -1.0f, 0.0f, 0.0f,
07151 0.0f, 0.0f, -1.0f, 0.0f,
07152 1.0f, 0.0f, 0.0f, 0.0f);
07153
07154 Transform pose = iter->second;
07155 if(!localTransform.isNull())
07156 {
07157 pose*=localTransform*optical_rotation_inv;
07158 }
07159 Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
07160
07161 out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
07162 out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
07163 out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
07164 out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
07165
07166 }
07167 }
07168
07169 fileList.close();
07170 fileOut.close();
07171
07172 QMessageBox::information(this,
07173 tr("Exporting cameras in Bundler format..."),
07174 tr("%1 cameras/images exported to directory \"%2\".%3")
07175 .arg(poses.size())
07176 .arg(path)
07177 .arg(ignoredCameras.size()>0?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(ignoredCameras.size()).arg(poses.size()):""));
07178 }
07179 else
07180 {
07181 fileOut.close();
07182 QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
07183 }
07184 }
07185 else
07186 {
07187 QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
07188 }
07189 }
07190 }
07191 else
07192 {
07193 QMessageBox::warning(this, tr("Exporting cameras..."), tr("No poses exported because of missing images. Try refreshing the cache (with clouds)."));
07194 }
07195 }
07196
07197 void MainWindow::resetOdometry()
07198 {
07199 UINFO("reset odometry");
07200 this->post(new OdometryResetEvent());
07201 }
07202
07203 void MainWindow::triggerNewMap()
07204 {
07205 UINFO("trigger a new map");
07206 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
07207 }
07208
07209 void MainWindow::dataRecorder()
07210 {
07211 if(_dataRecorder == 0)
07212 {
07213 QString path = QFileDialog::getSaveFileName(this, tr("Save to..."), _preferencesDialog->getWorkingDirectory()+"/output.db", "RTAB-Map database (*.db)");
07214 if(!path.isEmpty())
07215 {
07216 int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
07217
07218 if(r == QMessageBox::No || r == QMessageBox::Yes)
07219 {
07220 bool recordInRAM = r == QMessageBox::Yes;
07221
07222 _dataRecorder = new DataRecorder(this);
07223 _dataRecorder->setWindowFlags(Qt::Dialog);
07224 _dataRecorder->setAttribute(Qt::WA_DeleteOnClose, true);
07225 _dataRecorder->setWindowTitle(tr("Data recorder (%1)").arg(path));
07226
07227 if(_dataRecorder->init(path, recordInRAM))
07228 {
07229 this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed()));
07230 _dataRecorder->show();
07231 _dataRecorder->registerToEventsManager();
07232 if(_camera)
07233 {
07234 UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
07235 }
07236 _ui->actionData_recorder->setEnabled(false);
07237 }
07238 else
07239 {
07240 QMessageBox::warning(this, tr(""), tr("Cannot initialize the data recorder!"));
07241 UERROR("Cannot initialize the data recorder!");
07242 delete _dataRecorder;
07243 _dataRecorder = 0;
07244 }
07245 }
07246 }
07247 }
07248 else
07249 {
07250 UERROR("Only one recorder at the same time.");
07251 }
07252 }
07253
07254 void MainWindow::dataRecorderDestroyed()
07255 {
07256 _ui->actionData_recorder->setEnabled(true);
07257 _dataRecorder = 0;
07258 }
07259
07260
07261
07262
07263
07264
07265 void MainWindow::setMonitoringState(bool pauseChecked)
07266 {
07267 this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
07268 }
07269
07270
07271 void MainWindow::changeState(MainWindow::State newState)
07272 {
07273 bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
07274 _ui->label_source->setVisible(!monitoring);
07275 _ui->label_stats_source->setVisible(!monitoring);
07276 _ui->actionNew_database->setVisible(!monitoring);
07277 _ui->actionOpen_database->setVisible(!monitoring);
07278 _ui->actionClose_database->setVisible(!monitoring);
07279 _ui->actionEdit_database->setVisible(!monitoring);
07280 _ui->actionStart->setVisible(!monitoring);
07281 _ui->actionStop->setVisible(!monitoring);
07282 _ui->actionDump_the_memory->setVisible(!monitoring);
07283 _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
07284 _ui->actionGenerate_map->setVisible(!monitoring);
07285 _ui->actionUpdate_cache_from_database->setVisible(monitoring);
07286 _ui->actionData_recorder->setVisible(!monitoring);
07287 _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
07288 _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
07289 _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
07290 bool wasMonitoring = _state==kMonitoring || _state == kMonitoringPaused;
07291 if(wasMonitoring != monitoring)
07292 {
07293 _ui->toolBar->setVisible(!monitoring);
07294 _ui->toolBar->toggleViewAction()->setVisible(!monitoring);
07295 }
07296 QList<QAction*> actions = _ui->menuTools->actions();
07297 for(int i=0; i<actions.size(); ++i)
07298 {
07299 if(actions.at(i)->isSeparator())
07300 {
07301 actions.at(i)->setVisible(!monitoring);
07302 }
07303 }
07304 actions = _ui->menuFile->actions();
07305 if(actions.size()==16)
07306 {
07307 if(actions.at(2)->isSeparator())
07308 {
07309 actions.at(2)->setVisible(!monitoring);
07310 }
07311 else
07312 {
07313 UWARN("Menu File separators have not the same order.");
07314 }
07315 if(actions.at(12)->isSeparator())
07316 {
07317 actions.at(12)->setVisible(!monitoring);
07318 }
07319 else
07320 {
07321 UWARN("Menu File separators have not the same order.");
07322 }
07323 }
07324 else
07325 {
07326 UWARN("Menu File actions size has changed (%d)", actions.size());
07327 }
07328 actions = _ui->menuProcess->actions();
07329 if(actions.size()>=2)
07330 {
07331 if(actions.at(1)->isSeparator())
07332 {
07333 actions.at(1)->setVisible(!monitoring);
07334 }
07335 else
07336 {
07337 UWARN("Menu File separators have not the same order.");
07338 }
07339 }
07340 else
07341 {
07342 UWARN("Menu File separators have not the same order.");
07343 }
07344
07345 _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
07346
07347 switch (newState)
07348 {
07349 case kIdle:
07350 _ui->actionNew_database->setEnabled(true);
07351 _ui->actionOpen_database->setEnabled(true);
07352 _ui->actionClose_database->setEnabled(false);
07353 _ui->actionEdit_database->setEnabled(true);
07354 _ui->actionStart->setEnabled(false);
07355 _ui->actionPause->setEnabled(false);
07356 _ui->actionPause->setChecked(false);
07357 _ui->actionPause->setToolTip(tr("Pause"));
07358 _ui->actionStop->setEnabled(false);
07359 _ui->actionPause_on_match->setEnabled(true);
07360 _ui->actionPause_on_local_loop_detection->setEnabled(true);
07361 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
07362 _ui->actionDump_the_memory->setEnabled(false);
07363 _ui->actionDump_the_prediction_matrix->setEnabled(false);
07364 _ui->actionDelete_memory->setEnabled(false);
07365 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
07366 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07367 _ui->actionGenerate_map->setEnabled(false);
07368 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
07369 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07370 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07371 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
07372 #ifdef RTABMAP_OCTOMAP
07373 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
07374 #else
07375 _ui->actionExport_octomap->setEnabled(false);
07376 #endif
07377 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07378 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07379 _ui->actionDownload_all_clouds->setEnabled(false);
07380 _ui->actionDownload_graph->setEnabled(false);
07381 _ui->menuSelect_source->setEnabled(true);
07382 _ui->actionLabel_current_location->setEnabled(false);
07383 _ui->actionSend_goal->setEnabled(false);
07384 _ui->actionCancel_goal->setEnabled(false);
07385 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
07386 _ui->actionTrigger_a_new_map->setEnabled(false);
07387 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
07388 _ui->statusbar->clearMessage();
07389 _state = newState;
07390 _oneSecondTimer->stop();
07391 break;
07392
07393 case kApplicationClosing:
07394 case kClosing:
07395 _ui->actionStart->setEnabled(false);
07396 _ui->actionPause->setEnabled(false);
07397 _ui->actionStop->setEnabled(false);
07398 _state = newState;
07399 break;
07400
07401 case kInitializing:
07402 _ui->actionNew_database->setEnabled(false);
07403 _ui->actionOpen_database->setEnabled(false);
07404 _ui->actionClose_database->setEnabled(false);
07405 _ui->actionEdit_database->setEnabled(false);
07406 _state = newState;
07407 break;
07408
07409 case kInitialized:
07410 _ui->actionNew_database->setEnabled(false);
07411 _ui->actionOpen_database->setEnabled(false);
07412 _ui->actionClose_database->setEnabled(true);
07413 _ui->actionEdit_database->setEnabled(false);
07414 _ui->actionStart->setEnabled(true);
07415 _ui->actionPause->setEnabled(false);
07416 _ui->actionPause->setChecked(false);
07417 _ui->actionPause->setToolTip(tr("Pause"));
07418 _ui->actionStop->setEnabled(false);
07419 _ui->actionPause_on_match->setEnabled(true);
07420 _ui->actionPause_on_local_loop_detection->setEnabled(true);
07421 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
07422 _ui->actionDump_the_memory->setEnabled(true);
07423 _ui->actionDump_the_prediction_matrix->setEnabled(true);
07424 _ui->actionDelete_memory->setEnabled(_openedDatabasePath.isEmpty());
07425 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
07426 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07427 _ui->actionGenerate_map->setEnabled(true);
07428 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
07429 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07430 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07431 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
07432 #ifdef RTABMAP_OCTOMAP
07433 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
07434 #else
07435 _ui->actionExport_octomap->setEnabled(false);
07436 #endif
07437 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07438 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07439 _ui->actionDownload_all_clouds->setEnabled(true);
07440 _ui->actionDownload_graph->setEnabled(true);
07441 _ui->menuSelect_source->setEnabled(true);
07442 _ui->actionLabel_current_location->setEnabled(true);
07443 _ui->actionSend_goal->setEnabled(true);
07444 _ui->actionCancel_goal->setEnabled(true);
07445 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
07446 _ui->actionTrigger_a_new_map->setEnabled(true);
07447 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
07448 _ui->statusbar->clearMessage();
07449 _state = newState;
07450 _oneSecondTimer->stop();
07451 break;
07452
07453 case kStartingDetection:
07454 _ui->actionStart->setEnabled(false);
07455 _state = newState;
07456 break;
07457
07458 case kDetecting:
07459 _ui->actionNew_database->setEnabled(false);
07460 _ui->actionOpen_database->setEnabled(false);
07461 _ui->actionClose_database->setEnabled(false);
07462 _ui->actionEdit_database->setEnabled(false);
07463 _ui->actionStart->setEnabled(false);
07464 _ui->actionPause->setEnabled(true);
07465 _ui->actionPause->setChecked(false);
07466 _ui->actionPause->setToolTip(tr("Pause"));
07467 _ui->actionStop->setEnabled(true);
07468 _ui->actionPause_on_match->setEnabled(true);
07469 _ui->actionPause_on_local_loop_detection->setEnabled(true);
07470 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
07471 _ui->actionDump_the_memory->setEnabled(false);
07472 _ui->actionDump_the_prediction_matrix->setEnabled(false);
07473 _ui->actionDelete_memory->setEnabled(false);
07474 _ui->actionPost_processing->setEnabled(false);
07475 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
07476 _ui->actionGenerate_map->setEnabled(false);
07477 _ui->menuExport_poses->setEnabled(false);
07478 _ui->actionSave_point_cloud->setEnabled(false);
07479 _ui->actionView_high_res_point_cloud->setEnabled(false);
07480 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
07481 _ui->actionExport_octomap->setEnabled(false);
07482 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
07483 _ui->actionDepth_Calibration->setEnabled(false);
07484 _ui->actionDownload_all_clouds->setEnabled(false);
07485 _ui->actionDownload_graph->setEnabled(false);
07486 _ui->menuSelect_source->setEnabled(false);
07487 _ui->actionLabel_current_location->setEnabled(true);
07488 _ui->actionSend_goal->setEnabled(true);
07489 _ui->actionCancel_goal->setEnabled(true);
07490 _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
07491 _ui->actionTrigger_a_new_map->setEnabled(true);
07492 _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
07493 _ui->statusbar->showMessage(tr("Detecting..."));
07494 _state = newState;
07495 _ui->label_elapsedTime->setText("00:00:00");
07496 _elapsedTime->start();
07497 _oneSecondTimer->start();
07498
07499 _databaseUpdated = true;
07500
07501 if(_camera)
07502 {
07503 _camera->start();
07504 if(_imuThread)
07505 {
07506 _imuThread->start();
07507 }
07508 ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
07509 }
07510 break;
07511
07512 case kPaused:
07513 if(_state == kPaused)
07514 {
07515 _ui->actionPause->setToolTip(tr("Pause"));
07516 _ui->actionPause->setChecked(false);
07517 _ui->statusbar->showMessage(tr("Detecting..."));
07518 _ui->actionDump_the_memory->setEnabled(false);
07519 _ui->actionDump_the_prediction_matrix->setEnabled(false);
07520 _ui->actionDelete_memory->setEnabled(false);
07521 _ui->actionPost_processing->setEnabled(false);
07522 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
07523 _ui->actionGenerate_map->setEnabled(false);
07524 _ui->menuExport_poses->setEnabled(false);
07525 _ui->actionSave_point_cloud->setEnabled(false);
07526 _ui->actionView_high_res_point_cloud->setEnabled(false);
07527 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
07528 _ui->actionExport_octomap->setEnabled(false);
07529 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
07530 _ui->actionDepth_Calibration->setEnabled(false);
07531 _ui->actionDownload_all_clouds->setEnabled(false);
07532 _ui->actionDownload_graph->setEnabled(false);
07533 _state = kDetecting;
07534 _elapsedTime->start();
07535 _oneSecondTimer->start();
07536
07537 if(_camera)
07538 {
07539 _camera->start();
07540 if(_imuThread)
07541 {
07542 _imuThread->start();
07543 }
07544 ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
07545 }
07546 }
07547 else if(_state == kDetecting)
07548 {
07549 _ui->actionPause->setToolTip(tr("Continue (shift-click for step-by-step)"));
07550 _ui->actionPause->setChecked(true);
07551 _ui->statusbar->showMessage(tr("Paused..."));
07552 _ui->actionDump_the_memory->setEnabled(true);
07553 _ui->actionDump_the_prediction_matrix->setEnabled(true);
07554 _ui->actionDelete_memory->setEnabled(false);
07555 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
07556 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07557 _ui->actionGenerate_map->setEnabled(true);
07558 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
07559 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07560 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07561 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
07562 #ifdef RTABMAP_OCTOMAP
07563 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
07564 #else
07565 _ui->actionExport_octomap->setEnabled(false);
07566 #endif
07567 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07568 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07569 _ui->actionDownload_all_clouds->setEnabled(true);
07570 _ui->actionDownload_graph->setEnabled(true);
07571 _state = kPaused;
07572 _oneSecondTimer->stop();
07573
07574
07575 if(_camera)
07576 {
07577 if(_imuThread)
07578 {
07579 _imuThread->join(true);
07580 }
07581 _camera->join(true);
07582 }
07583 }
07584 break;
07585 case kMonitoring:
07586 _ui->actionPause->setEnabled(true);
07587 _ui->actionPause->setChecked(false);
07588 _ui->actionPause->setToolTip(tr("Pause"));
07589 _ui->actionPause_on_match->setEnabled(true);
07590 _ui->actionPause_on_local_loop_detection->setEnabled(true);
07591 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
07592 _ui->actionReset_Odometry->setEnabled(true);
07593 _ui->actionPost_processing->setEnabled(false);
07594 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
07595 _ui->menuExport_poses->setEnabled(false);
07596 _ui->actionSave_point_cloud->setEnabled(false);
07597 _ui->actionView_high_res_point_cloud->setEnabled(false);
07598 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
07599 _ui->actionExport_octomap->setEnabled(false);
07600 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
07601 _ui->actionDepth_Calibration->setEnabled(false);
07602 _ui->actionDelete_memory->setEnabled(true);
07603 _ui->actionDownload_all_clouds->setEnabled(true);
07604 _ui->actionDownload_graph->setEnabled(true);
07605 _ui->actionTrigger_a_new_map->setEnabled(true);
07606 _ui->actionLabel_current_location->setEnabled(true);
07607 _ui->actionSend_goal->setEnabled(true);
07608 _ui->actionCancel_goal->setEnabled(true);
07609 _ui->statusbar->showMessage(tr("Monitoring..."));
07610 _state = newState;
07611 _elapsedTime->start();
07612 _oneSecondTimer->start();
07613 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResume));
07614 break;
07615 case kMonitoringPaused:
07616 _ui->actionPause->setToolTip(tr("Continue"));
07617 _ui->actionPause->setChecked(true);
07618 _ui->actionPause->setEnabled(true);
07619 _ui->actionPause_on_match->setEnabled(true);
07620 _ui->actionPause_on_local_loop_detection->setEnabled(true);
07621 _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
07622 _ui->actionReset_Odometry->setEnabled(true);
07623 _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
07624 _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07625 _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
07626 _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07627 _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
07628 _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
07629 #ifdef RTABMAP_OCTOMAP
07630 _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
07631 #else
07632 _ui->actionExport_octomap->setEnabled(false);
07633 #endif
07634 _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07635 _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
07636 _ui->actionDelete_memory->setEnabled(true);
07637 _ui->actionDownload_all_clouds->setEnabled(true);
07638 _ui->actionDownload_graph->setEnabled(true);
07639 _ui->actionTrigger_a_new_map->setEnabled(true);
07640 _ui->actionLabel_current_location->setEnabled(true);
07641 _ui->actionSend_goal->setEnabled(true);
07642 _ui->actionCancel_goal->setEnabled(true);
07643 _ui->statusbar->showMessage(tr("Monitoring paused..."));
07644 _state = newState;
07645 _oneSecondTimer->stop();
07646 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause));
07647 break;
07648 default:
07649 break;
07650 }
07651
07652 }
07653
07654 }