MainWindow.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/MainWindow.h"
29 
30 #include "ui_mainWindow.h"
31 
32 #include "rtabmap/core/CameraRGB.h"
33 #include "rtabmap/core/CameraStereo.h"
34 #include "rtabmap/core/CameraThread.h"
35 #include "rtabmap/core/IMUThread.h"
36 #include "rtabmap/core/CameraEvent.h"
37 #include "rtabmap/core/DBReader.h"
38 #include "rtabmap/core/Parameters.h"
39 #include "rtabmap/core/ParamEvent.h"
40 #include "rtabmap/core/Signature.h"
41 #include "rtabmap/core/Memory.h"
42 #include "rtabmap/core/DBDriver.h"
43 #include "rtabmap/core/RegistrationVis.h"
44 #include "rtabmap/core/OccupancyGrid.h"
45 #include "rtabmap/core/GainCompensator.h"
46 #include "rtabmap/core/Recovery.h"
47 #include "rtabmap/core/util2d.h"
48 
49 #include "rtabmap/gui/ImageView.h"
50 #include "rtabmap/gui/KeypointItem.h"
51 #include "rtabmap/gui/DataRecorder.h"
52 #include "rtabmap/gui/DatabaseViewer.h"
53 #include "rtabmap/gui/PdfPlot.h"
54 #include "rtabmap/gui/StatsToolBox.h"
55 #include "rtabmap/gui/ProgressDialog.h"
56 #include "rtabmap/gui/CloudViewer.h"
57 #include "rtabmap/gui/LoopClosureViewer.h"
58 #include "rtabmap/gui/ExportCloudsDialog.h"
59 #include "rtabmap/gui/ExportBundlerDialog.h"
60 #include "rtabmap/gui/AboutDialog.h"
61 #include "rtabmap/gui/PostProcessingDialog.h"
62 #include "rtabmap/gui/DepthCalibrationDialog.h"
63 #include "rtabmap/gui/RecoveryState.h"
64 
65 #include <rtabmap/utilite/UStl.h>
66 #include <rtabmap/utilite/ULogger.h>
67 #include <rtabmap/utilite/UEventsManager.h>
68 #include <rtabmap/utilite/UFile.h>
69 #include <rtabmap/utilite/UConversion.h>
70 #include "rtabmap/utilite/UPlot.h"
71 #include "rtabmap/utilite/UCv2Qt.h"
72 
73 #include <QtGui/QCloseEvent>
74 #include <QtGui/QPixmap>
75 #include <QtCore/QDir>
76 #include <QtCore/QFile>
77 #include <QtCore/QTextStream>
78 #include <QtCore/QFileInfo>
79 #include <QMessageBox>
80 #include <QFileDialog>
81 #include <QGraphicsEllipseItem>
82 #include <QDockWidget>
83 #include <QtCore/QBuffer>
84 #include <QtCore/QTimer>
85 #include <QtCore/QTime>
86 #include <QActionGroup>
87 #include <QtCore/QThread>
88 #include <QtGui/QDesktopServices>
89 #include <QtCore/QStringList>
90 #include <QtCore/QProcess>
91 #include <QSplashScreen>
92 #include <QInputDialog>
93 #include <QToolButton>
94 
95 //RGB-D stuff
96 #include "rtabmap/core/CameraRGBD.h"
97 #include "rtabmap/core/Odometry.h"
98 #include "rtabmap/core/OdometryThread.h"
99 #include "rtabmap/core/OdometryEvent.h"
100 #include "rtabmap/core/util3d.h"
101 #include "rtabmap/core/util3d_transforms.h"
102 #include "rtabmap/core/util3d_filtering.h"
103 #include "rtabmap/core/util3d_mapping.h"
104 #include "rtabmap/core/util3d_surface.h"
105 #include "rtabmap/core/util3d_registration.h"
106 #include "rtabmap/core/Optimizer.h"
107 #include "rtabmap/core/OptimizerCVSBA.h"
108 #include "rtabmap/core/Graph.h"
109 #include "rtabmap/core/RegistrationIcp.h"
110 #include <pcl/visualization/cloud_viewer.h>
111 #include <pcl/common/transforms.h>
112 #include <pcl/common/common.h>
113 #include <pcl/io/pcd_io.h>
114 #include <pcl/io/ply_io.h>
115 #include <pcl/filters/filter.h>
116 #include <pcl/search/kdtree.h>
117 
118 #ifdef RTABMAP_OCTOMAP
119 #include <rtabmap/core/OctoMap.h>
120 #endif
121 
122 #define LOG_FILE_NAME "LogRtabmap.txt"
123 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
124 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
125 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m"
126 
127 using namespace rtabmap;
128 
129 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
130 
131 
132 namespace rtabmap {
133 
134 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool showSplashScreen) :
135  QMainWindow(parent),
136  _ui(0),
137  _state(kIdle),
138  _camera(0),
139  _odomThread(0),
140  _imuThread(0),
141  _preferencesDialog(0),
142  _aboutDialog(0),
143  _exportCloudsDialog(0),
144  _exportBundlerDialog(0),
145  _dataRecorder(0),
146  _lastId(0),
147  _firstStamp(0.0f),
148  _processingStatistics(false),
149  _processingDownloadedMap(false),
150  _recovering(false),
151  _odometryReceived(false),
152  _newDatabasePath(""),
153  _newDatabasePathOutput(""),
154  _openedDatabasePath(""),
155  _databaseUpdated(false),
156  _odomImageShow(true),
157  _odomImageDepthShow(false),
158  _savedMaximized(false),
159  _waypointsIndex(0),
160  _cachedMemoryUsage(0),
161  _createdCloudsMemoryUsage(0),
162  _occupancyGrid(0),
163  _octomap(0),
164  _odometryCorrection(Transform::getIdentity()),
165  _processingOdometry(false),
166  _oneSecondTimer(0),
167  _elapsedTime(0),
168  _posteriorCurve(0),
169  _likelihoodCurve(0),
170  _rawLikelihoodCurve(0),
171  _exportPosesFrame(0),
172  _autoScreenCaptureOdomSync(false),
173  _autoScreenCaptureRAM(false),
174  _autoScreenCapturePNG(false),
175  _firstCall(true),
176  _progressCanceled(false)
177 {
178  ULogger::registerCurrentThread("MainWindow");
179  UDEBUG("");
180 
181  initGuiResource();
182 
183  QSplashScreen * splash = 0;
184  if (showSplashScreen)
185  {
186  QPixmap pixmap(":images/RTAB-Map.png");
187  splash = new QSplashScreen(pixmap);
188  splash->show();
189  splash->showMessage(tr("Loading..."));
190  QApplication::processEvents();
191  }
192 
193  // Create dialogs
194  _aboutDialog = new AboutDialog(this);
195  _aboutDialog->setObjectName("AboutDialog");
196  _exportCloudsDialog = new ExportCloudsDialog(this);
197  _exportCloudsDialog->setObjectName("ExportCloudsDialog");
198  _exportBundlerDialog = new ExportBundlerDialog(this);
199  _exportBundlerDialog->setObjectName("ExportBundlerDialog");
200  _postProcessingDialog = new PostProcessingDialog(this);
201  _postProcessingDialog->setObjectName("PostProcessingDialog");
202  _depthCalibrationDialog = new DepthCalibrationDialog(this);
203  _depthCalibrationDialog->setObjectName("DepthCalibrationDialog");
204 
205  _ui = new Ui_mainWindow();
206  UDEBUG("Setup ui...");
207  _ui->setupUi(this);
208 
209  // Add cloud viewers
210  // Note that we add them here manually because there is a crash issue
211  // when adding them in a DockWidget of the *.ui file. The cloud viewer is
212  // created in a widget which is not yet linked to main window when the CloudViewer constructor
213  // is called (see order in generated ui file). VTK needs to get the top
214  // level window at the time CloudViewer is created, otherwise it may crash on some systems.
215  _cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
216  _cloudViewer->setObjectName("widget_cloudViewer");
217  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
218  _loopClosureViewer = new LoopClosureViewer(_ui->layout_loopClosureViewer);
219  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
220  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
221  UDEBUG("Setup ui... end");
222 
223  QString title("RTAB-Map[*]");
224  this->setWindowTitle(title);
225  this->setWindowIconText(tr("RTAB-Map"));
226  this->setObjectName("MainWindow");
227 
228  //Setup dock widgets position if it is the first time the application is started.
229  setDefaultViews();
230 
231  _ui->widget_mainWindow->setVisible(false);
232 
233  if(prefDialog)
234  {
235  _preferencesDialog = prefDialog;
236  _preferencesDialog->setParent(this, Qt::Dialog);
237  }
238  else // Default dialog
239  {
240  _preferencesDialog = new PreferencesDialog(this);
241  }
242 
243  _preferencesDialog->setObjectName("PreferencesDialog");
244  _preferencesDialog->init();
245 
246  // Restore window geometry
247  bool statusBarShown = false;
248  _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown);
249  _preferencesDialog->loadWindowGeometry(_preferencesDialog);
250  _preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
251  _preferencesDialog->loadWindowGeometry(_exportBundlerDialog);
252  _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
253  _preferencesDialog->loadWindowGeometry(_depthCalibrationDialog);
254  _preferencesDialog->loadWindowGeometry(_aboutDialog);
255  setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
256 
257  ParametersMap parameters = _preferencesDialog->getAllParameters();
258  _occupancyGrid = new OccupancyGrid();
259 #ifdef RTABMAP_OCTOMAP
260  _octomap = new OctoMap(parameters);
261 #endif
262 
263  // Timer
264  _oneSecondTimer = new QTimer(this);
265  _oneSecondTimer->setInterval(1000);
266  _elapsedTime = new QTime();
267  _ui->label_elapsedTime->setText("00:00:00");
268  connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
269  _logEventTime = new QTime();
270  _logEventTime->start();
271 
272  //Graphics scenes
273  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
274  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
275  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
276  _ui->imageView_odometry->setAlpha(200);
277  _preferencesDialog->loadWidgetState(_ui->imageView_source);
278  _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
279  _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
280  _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
281 
282  _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
283  _ui->posteriorPlot->addCurve(_posteriorCurve, false);
284  _ui->posteriorPlot->showLegend(false);
285  _ui->posteriorPlot->setFixedYAxis(0,1);
286  UPlotCurveThreshold * tc;
287  tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
288  connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
289 
290  _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
291  _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
292  _ui->likelihoodPlot->showLegend(false);
293 
294  _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
295  _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
296  _ui->rawLikelihoodPlot->showLegend(false);
297 
298  _progressDialog = new ProgressDialog(this);
299  _progressDialog->setMinimumWidth(800);
300  connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancelProgress()));
301 
302  connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
303 
304  //connect stuff
305  connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
306  qRegisterMetaType<MainWindow::State>("MainWindow::State");
307  connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
308  connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
309  qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
310  connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
311  qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
312  connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
313  connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
314  connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
315 
316  // Dock Widget view actions (Menu->Window)
317  _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
318  _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
319  _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
320  _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
321  _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
322  _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
323  _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
324  _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
325  _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
326  _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
327  _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
328  _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
329  _ui->toolBar->setWindowTitle(tr("File toolbar"));
330  _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
331  _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
332  QAction * a = _ui->menuShow_view->addAction("Progress dialog");
333  a->setCheckable(false);
334  connect(a, SIGNAL(triggered(bool)), _progressDialog, SLOT(show()));
335  QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar");
336  statusBarAction->setCheckable(true);
337  statusBarAction->setChecked(statusBarShown);
338  connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool)));
339 
340  // connect actions with custom slots
341  connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
342  connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
343  connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
344  connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
345  connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
346  connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
347  connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
348  connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
349  connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
350  connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
351  connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
352  connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
353  connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
354  connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
355  connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
356  connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
357  connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
358  connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
359  connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
360  connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
361  connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
362  connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
363  connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
364  connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
365  connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
366  connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
367  connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
368  connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
369  connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
370  connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
371  connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
372  connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
373  connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
374  connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
375  connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
376  connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
377  connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
378  connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
379  connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
380  connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
381  connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
382  connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom()));
383  connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
384  connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
385  connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages()));
386  connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat()));
387  connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap()));
388  connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
389  connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
390  connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
391  connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
392  connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
393  connect(_ui->actionDepth_Calibration, SIGNAL(triggered()), this, SLOT(depthCalibration()));
394 
395  _ui->actionPause->setShortcut(Qt::Key_Space);
396  _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
397  // Qt5 issue, we should explicitly add actions not in
398  // menu bar to have shortcut working
399  this->addAction(_ui->actionSave_GUI_config);
400  _ui->actionReset_Odometry->setEnabled(false);
401  _ui->actionPost_processing->setEnabled(false);
402  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
403 
404  QToolButton* toolButton = new QToolButton(this);
405  toolButton->setMenu(_ui->menuSelect_source);
406  toolButton->setPopupMode(QToolButton::InstantPopup);
407  toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
408  toolButton->setToolTip("Select sensor driver");
409  _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
410 
411 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
412  connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
413 #else
414  _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
415 #endif
416 
417  //Settings menu
418  connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
419  connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
420  connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
421  connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
422  connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
423  connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
424  connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
425  connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
426  connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
427  connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
428  connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
429  connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
430  connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
431  connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
432  connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
433  connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
434  connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
435  connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
436  connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
437  connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
438  _ui->actionFreenect->setEnabled(CameraFreenect::available());
439  _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
440  _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
441  _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
442  _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
443  _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
444  _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
445  _ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
446  _ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
447  _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
448  _ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
449  _ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
450  _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
451  _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
452  _ui->actionStereoZed->setEnabled(CameraStereoZed::available());
453  this->updateSelectSourceMenu();
454 
455  connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
456 
457  QActionGroup * modeGrp = new QActionGroup(this);
458  modeGrp->addAction(_ui->actionSLAM_mode);
459  modeGrp->addAction(_ui->actionLocalization_mode);
460  _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
461  _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
462  connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
463  connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
464  connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
465 
466  // Settings changed
467  qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
468  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
469  qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
470  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
471  // config GUI modified
472  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
473  if(prefDialog == 0)
474  {
475  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
476  }
477  connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
478  connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
479  connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
480  connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
481  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
482  connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
483  connect(_exportBundlerDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
484  connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
485  connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
486  connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
487  connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
488  connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
489  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
490  for(int i=0; i<dockWidgets.size(); ++i)
491  {
492  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
493  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
494  }
495  connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
496  // catch resize events
497  _ui->dockWidget_posterior->installEventFilter(this);
498  _ui->dockWidget_likelihood->installEventFilter(this);
499  _ui->dockWidget_rawlikelihood->installEventFilter(this);
500  _ui->dockWidget_statsV2->installEventFilter(this);
501  _ui->dockWidget_console->installEventFilter(this);
502  _ui->dockWidget_loopClosureViewer->installEventFilter(this);
503  _ui->dockWidget_mapVisibility->installEventFilter(this);
504  _ui->dockWidget_graphViewer->installEventFilter(this);
505  _ui->dockWidget_odometry->installEventFilter(this);
506  _ui->dockWidget_cloudViewer->installEventFilter(this);
507  _ui->dockWidget_imageView->installEventFilter(this);
508 
509  // more connects...
510  _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
511  _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
512  _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
513  connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
514  connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
515  connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
516  connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
517  connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
518  connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
519 
520  // Statistics from the detector
521  qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
522  connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
523 
524  qRegisterMetaType<rtabmap::CameraInfo>("rtabmap::CameraInfo");
525  connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo)));
526 
527  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
528  connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool)));
529 
530  connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
531 
532  // Apply state
533  this->changeState(kIdle);
534  this->applyPrefSettings(PreferencesDialog::kPanelAll);
535 
536  _ui->statsToolBox->setNewFigureMaxItems(50);
537  _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
538  _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
539  _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
540  _cloudViewer->setBackfaceCulling(true, false);
541  _preferencesDialog->loadWidgetState(_cloudViewer);
542 
543  //dialog states
544  _preferencesDialog->loadWidgetState(_exportCloudsDialog);
545  _preferencesDialog->loadWidgetState(_exportBundlerDialog);
546  _preferencesDialog->loadWidgetState(_postProcessingDialog);
547  _preferencesDialog->loadWidgetState(_depthCalibrationDialog);
548 
549  if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
550  {
551  const std::map<std::string, float> & statistics = Statistics::defaultData();
552  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
553  {
554  // Don't add Gt panels yet if we don't know if we will receive Gt values.
555  if(!QString((*iter).first.c_str()).contains("Gt/"))
556  {
557  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
558  }
559  }
560  }
561  // Specific MainWindow
562  _ui->statsToolBox->updateStat("Planning/From/", false);
563  _ui->statsToolBox->updateStat("Planning/Time/ms", false);
564  _ui->statsToolBox->updateStat("Planning/Goal/", false);
565  _ui->statsToolBox->updateStat("Planning/Poses/", false);
566  _ui->statsToolBox->updateStat("Planning/Length/m", false);
567 
568  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false);
569  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false);
570  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false);
571  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false);
572  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false);
573 
574  _ui->statsToolBox->updateStat("Odometry/ID/", false);
575  _ui->statsToolBox->updateStat("Odometry/Features/", false);
576  _ui->statsToolBox->updateStat("Odometry/Matches/", false);
577  _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
578  _ui->statsToolBox->updateStat("Odometry/Inliers/", false);
579  _ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
580  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
581  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
582  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
583  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
584  _ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
585  _ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
586  _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
587  _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
588  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
589  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
590  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
591  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
592  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
593  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
594  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
595  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
596  _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
597  _ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
598  _ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
599  _ui->statsToolBox->updateStat("Odometry/Distance/m", false);
600  _ui->statsToolBox->updateStat("Odometry/Tx/m", false);
601  _ui->statsToolBox->updateStat("Odometry/Ty/m", false);
602  _ui->statsToolBox->updateStat("Odometry/Tz/m", false);
603  _ui->statsToolBox->updateStat("Odometry/Troll/deg", false);
604  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", false);
605  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", false);
606  _ui->statsToolBox->updateStat("Odometry/Px/m", false);
607  _ui->statsToolBox->updateStat("Odometry/Py/m", false);
608  _ui->statsToolBox->updateStat("Odometry/Pz/m", false);
609  _ui->statsToolBox->updateStat("Odometry/Proll/deg", false);
610  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", false);
611  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", false);
612 
613  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false);
614  _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false);
615  _ui->statsToolBox->updateStat("GUI/Graph Update/ms", false);
616 #ifdef RTABMAP_OCTOMAP
617  _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
618  _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
619 #endif
620  _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
621  _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
622  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
623  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
624  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
625 #ifdef RTABMAP_OCTOMAP
626  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false);
627 #endif
628 
629  this->loadFigures();
630  connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
631 
632  // update loop closure viewer parameters
633  _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
634  _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
635 
636  if (splash)
637  {
638  splash->close();
639  delete splash;
640  }
641 
642  this->setFocus();
643 
644  UDEBUG("");
645 }
646 
647 MainWindow::~MainWindow()
648 {
649  UDEBUG("");
650  this->stopDetection();
651  delete _ui;
652  delete _elapsedTime;
653 #ifdef RTABMAP_OCTOMAP
654  delete _octomap;
655 #endif
656  delete _occupancyGrid;
657  UDEBUG("");
658 }
659 
660 void MainWindow::setupMainLayout(bool vertical)
661 {
662  if(vertical)
663  {
664  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
665  }
666  else if(!vertical)
667  {
668  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
669  }
670 }
671 
672 void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
673 {
674  UASSERT(cloudViewer);
675  delete _cloudViewer;
676  _cloudViewer = cloudViewer;
677  _cloudViewer->setParent(_ui->layout_cloudViewer);
678  _cloudViewer->setObjectName("widget_cloudViewer");
679  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
680 
681  _cloudViewer->setBackfaceCulling(true, false);
682  _preferencesDialog->loadWidgetState(_cloudViewer);
683 
684  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
685 }
686 void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
687 {
688  UASSERT(loopClosureViewer);
689  delete _loopClosureViewer;
690  _loopClosureViewer = loopClosureViewer;
691  _loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
692  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
693  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
694 }
695 
696 void MainWindow::closeEvent(QCloseEvent* event)
697 {
698  // Try to close all children
699  /*QList<QMainWindow *> windows = this->findChildren<QMainWindow *>();
700  for(int i=0; i<windows.size(); i++) {
701  if(!windows[i]->close()) {
702  event->setAccepted(false);
703  return;
704  }
705  }*/
706  UDEBUG("");
707  bool processStopped = true;
708  if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
709  {
710  this->stopDetection();
711  if(_state == kInitialized)
712  {
713  if(this->closeDatabase())
714  {
715  this->changeState(kApplicationClosing);
716  }
717  }
718  if(_state != kIdle)
719  {
720  processStopped = false;
721  }
722  }
723 
724  if(processStopped)
725  {
726  //write settings before quit?
727  bool save = false;
728  if(this->isWindowModified())
729  {
730  QMessageBox::Button b=QMessageBox::question(this,
731  tr("RTAB-Map"),
732  tr("There are unsaved changed settings. Save them?"),
733  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
734  if(b == QMessageBox::Save)
735  {
736  save = true;
737  }
738  else if(b != QMessageBox::Discard)
739  {
740  event->ignore();
741  return;
742  }
743  }
744 
745  if(save)
746  {
747  saveConfigGUI();
748  }
749 
750  _ui->statsToolBox->closeFigures();
751 
752  _ui->dockWidget_imageView->close();
753  _ui->dockWidget_likelihood->close();
754  _ui->dockWidget_rawlikelihood->close();
755  _ui->dockWidget_posterior->close();
756  _ui->dockWidget_statsV2->close();
757  _ui->dockWidget_console->close();
758  _ui->dockWidget_cloudViewer->close();
759  _ui->dockWidget_loopClosureViewer->close();
760  _ui->dockWidget_mapVisibility->close();
761  _ui->dockWidget_graphViewer->close();
762  _ui->dockWidget_odometry->close();
763 
764  if(_camera)
765  {
766  UERROR("Camera must be already deleted here!");
767  delete _camera;
768  _camera = 0;
769  if(_imuThread)
770  {
771  delete _imuThread;
772  _imuThread = 0;
773  }
774  }
775  if(_odomThread)
776  {
777  UERROR("OdomThread must be already deleted here!");
778  delete _odomThread;
779  _odomThread = 0;
780  }
781  event->accept();
782  }
783  else
784  {
785  event->ignore();
786  }
787  UDEBUG("");
788 }
789 
790 bool MainWindow::handleEvent(UEvent* anEvent)
791 {
792  if(anEvent->getClassName().compare("RtabmapEvent") == 0)
793  {
794  RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
795  Statistics stats = rtabmapEvent->getStats();
796  int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
797  int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
798  bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
799  float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
800  if((stats.loopClosureId() > 0 &&
801  _ui->actionPause_on_match->isChecked())
802  ||
803  (stats.loopClosureId() == 0 &&
804  highestHypothesisId > 0 &&
805  highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
806  _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
807  rejectedHyp)
808  ||
809  (proximityClosureId > 0 &&
810  _ui->actionPause_on_local_loop_detection->isChecked()))
811  {
812  if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
813  {
814  if(_preferencesDialog->beepOnPause())
815  {
816  QMetaObject::invokeMethod(this, "beep");
817  }
818  this->pauseDetection();
819  }
820  }
821 
822  if(!_processingDownloadedMap)
823  {
824  _processingStatistics = true;
825  Q_EMIT statsReceived(stats);
826  }
827  }
828  else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
829  {
830  if(!_recovering)
831  {
832  RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
833  Q_EMIT rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
834  }
835  }
836  else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
837  {
838  RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
839  Q_EMIT rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
840  }
841  else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
842  {
843  RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
844  Q_EMIT rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
845  }
846  else if(anEvent->getClassName().compare("RtabmapLabelErrorEvent") == 0)
847  {
848  RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
849  Q_EMIT rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
850  }
851  else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
852  {
853  Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode());
854  }
855  else if(anEvent->getClassName().compare("CameraEvent") == 0)
856  {
857  CameraEvent * cameraEvent = (CameraEvent*)anEvent;
858  if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
859  {
860  if(_preferencesDialog->beepOnPause())
861  {
862  QMetaObject::invokeMethod(this, "beep");
863  }
864  Q_EMIT noMoreImagesReceived();
865  }
866  else
867  {
868  Q_EMIT cameraInfoReceived(cameraEvent->info());
869  if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
870  {
871  OdometryInfo odomInfo;
872  odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
873  if (!_processingOdometry && !_processingStatistics)
874  {
875  _processingOdometry = true; // if we receive too many odometry events!
876  OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo);
877  Q_EMIT odometryReceived(tmp, false);
878  }
879  else
880  {
881  // we receive too many odometry events! ignore them
882  }
883  }
884  }
885  }
886  else if(anEvent->getClassName().compare("OdometryEvent") == 0)
887  {
888  OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
889  if(!_processingOdometry && !_processingStatistics)
890  {
891  _processingOdometry = true; // if we receive too many odometry events!
892  Q_EMIT odometryReceived(*odomEvent, false);
893  }
894  else
895  {
896  // we receive too many odometry events! just send without data
897  SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
898  data.setCameraModels(odomEvent->data().cameraModels());
899  data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
900  data.setGroundTruth(odomEvent->data().groundTruth());
901  OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
902  Q_EMIT odometryReceived(tmp, true);
903  }
904  }
905  else if(anEvent->getClassName().compare("ULogEvent") == 0)
906  {
907  ULogEvent * logEvent = (ULogEvent*)anEvent;
908  if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
909  {
910  QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
911  // The timer prevents multiple calls to pauseDetection() before the state can be changed
912  if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
913  {
914  _logEventTime->start();
915  if(_preferencesDialog->beepOnPause())
916  {
917  QMetaObject::invokeMethod(this, "beep");
918  }
919  pauseDetection();
920  }
921  }
922  }
923  return false;
924 }
925 
926 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
927 {
928  if(_firstStamp == 0.0)
929  {
930  _firstStamp = info.stamp;
931  }
932  _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
933  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
934  _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
935  _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
936  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
937  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
938  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
939  _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
940  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
941 
942  Q_EMIT(cameraInfoProcessed());
943 }
944 
945 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
946 {
947  if(_firstStamp == 0.0)
948  {
949  _firstStamp = odom.data().stamp();
950  }
951 
952  UDEBUG("");
953  _processingOdometry = true;
954  UTimer time;
955  // Process Data
956 
957  // Set color code as tooltip
958  if(_ui->imageView_odometry->toolTip().isEmpty())
959  {
960  _ui->imageView_odometry->setToolTip(
961  "Background Color Code:\n"
962  " Dark Red = Odometry Lost\n"
963  " Dark Yellow = Low Inliers\n"
964  "Feature Color code:\n"
965  " Green = Inliers\n"
966  " Yellow = Not matched features from previous frame(s)\n"
967  " Red = Outliers");
968  }
969 
970  Transform pose = odom.pose();
971  bool lost = false;
972  bool lostStateChanged = false;
973 
974  if(pose.isNull())
975  {
976  UDEBUG("odom lost"); // use last pose
977  lostStateChanged = _cloudViewer->getBackgroundColor() != Qt::darkRed;
978  _cloudViewer->setBackgroundColor(Qt::darkRed);
979  _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
980 
981  pose = _lastOdomPose;
982  lost = true;
983  }
984  else if(odom.info().reg.inliers>0 &&
985  _preferencesDialog->getOdomQualityWarnThr() &&
986  odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
987  {
988  UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
989  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
990  _cloudViewer->setBackgroundColor(Qt::darkYellow);
991  _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
992  }
993  else
994  {
995  UDEBUG("odom ok");
996  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
997  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
998  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
999  }
1000 
1001  if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
1002  {
1003  _lastOdomPose = pose;
1004  }
1005 
1006  if(_ui->dockWidget_cloudViewer->isVisible())
1007  {
1008  bool cloudUpdated = false;
1009  bool scanUpdated = false;
1010  bool featuresUpdated = false;
1011  if(!pose.isNull())
1012  {
1013  // 3d cloud
1014  if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
1015  odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
1016  !odom.data().depthOrRightRaw().empty() &&
1017  (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
1018  _preferencesDialog->isCloudsShown(1))
1019  {
1020  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1021  pcl::IndicesPtr indices(new std::vector<int>);
1022  cloud = util3d::cloudRGBFromSensorData(odom.data(),
1023  _preferencesDialog->getCloudDecimation(1),
1024  _preferencesDialog->getCloudMaxDepth(1),
1025  _preferencesDialog->getCloudMinDepth(1),
1026  indices.get(),
1027  _preferencesDialog->getAllParameters(),
1028  _preferencesDialog->getCloudRoiRatios(1));
1029  if(indices->size())
1030  {
1031  cloud = util3d::transformPointCloud(cloud, pose);
1032 
1033  if(_preferencesDialog->isCloudMeshing())
1034  {
1035  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
1036  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
1037  output = util3d::extractIndices(cloud, indices, false, true);
1038 
1039  // Fast organized mesh
1040  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
1041  if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
1042  {
1043  viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
1044  viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
1045  viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
1046  }
1047  else if(!odom.data().stereoCameraModel().localTransform().isNull())
1048  {
1049  viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
1050  viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
1051  viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
1052  }
1053  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
1054  output,
1055  _preferencesDialog->getCloudMeshingAngle(),
1056  _preferencesDialog->isCloudMeshingQuad(),
1057  _preferencesDialog->getCloudMeshingTriangleSize(),
1058  Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
1059  if(polygons.size())
1060  {
1061  if(_preferencesDialog->isCloudMeshingTexture() && !odom.data().imageRaw().empty())
1062  {
1063  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
1064  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1065  textureMesh->tex_polygons.push_back(polygons);
1066  int w = cloud->width;
1067  int h = cloud->height;
1068  UASSERT(w > 1 && h > 1);
1069  textureMesh->tex_coordinates.resize(1);
1070  int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
1071  textureMesh->tex_coordinates[0].resize(nPoints);
1072  for(int i=0; i<nPoints; ++i)
1073  {
1074  //uv
1075  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
1076  float(i % w) / float(w), // u
1077  float(h - i / w) / float(h)); // v
1078  }
1079 
1080  pcl::TexMaterial mesh_material;
1081  mesh_material.tex_d = 1.0f;
1082  mesh_material.tex_Ns = 75.0f;
1083  mesh_material.tex_illum = 1;
1084 
1085  mesh_material.tex_name = "material_odom";
1086  mesh_material.tex_file = "";
1087  textureMesh->tex_materials.push_back(mesh_material);
1088 
1089  if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, odom.data().imageRaw(), _odometryCorrection))
1090  {
1091  UERROR("Adding cloudOdom to viewer failed!");
1092  }
1093  }
1094  else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
1095  {
1096  UERROR("Adding cloudOdom to viewer failed!");
1097  }
1098  }
1099  }
1100  else
1101  {
1102  if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
1103  {
1104  UERROR("Adding cloudOdom to viewer failed!");
1105  }
1106  }
1107  _cloudViewer->setCloudVisibility("cloudOdom", true);
1108  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
1109  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
1110  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
1111 
1112  cloudUpdated = true;
1113  }
1114  }
1115 
1116  if(_preferencesDialog->isScansShown(1))
1117  {
1118  // F2M: scan local map
1119  if(!odom.info().localScanMap.isEmpty())
1120  {
1121  if(!lost)
1122  {
1123  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
1124  cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
1125  bool scanAdded = _cloudViewer->getAddedClouds().contains("scanMapOdom");
1126  if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
1127  {
1128  UERROR("Adding scanMapOdom to viewer failed!");
1129  }
1130  else
1131  {
1132  _cloudViewer->setCloudVisibility("scanMapOdom", true);
1133  _cloudViewer->setCloudColorIndex("scanMapOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1134  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
1135  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
1136  }
1137  }
1138  scanUpdated = true;
1139  }
1140  // scan cloud
1141  if(!odom.data().laserScanRaw().isEmpty())
1142  {
1143  LaserScan scan = odom.data().laserScanRaw();
1144 
1145  if(_preferencesDialog->getDownsamplingStepScan(1) > 1 ||
1146  _preferencesDialog->getScanMaxRange(1) > 0.0f ||
1147  _preferencesDialog->getScanMinRange(1) > 0.0f)
1148  {
1149  scan = util3d::commonFiltering(scan,
1150  _preferencesDialog->getDownsamplingStepScan(1),
1151  _preferencesDialog->getScanMinRange(1),
1152  _preferencesDialog->getScanMaxRange(1));
1153  }
1154 
1155  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
1156  cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
1157  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1158  {
1159  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1160  }
1161 
1162  bool scanAdded = !_cloudViewer->getAddedClouds().contains("scanOdom");
1163  if(!_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
1164  {
1165  UERROR("Adding scanOdom to viewer failed!");
1166  }
1167  else
1168  {
1169  _cloudViewer->setCloudVisibility("scanOdom", true);
1170  _cloudViewer->setCloudColorIndex("scanOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1171  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
1172  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
1173  scanUpdated = true;
1174  }
1175  }
1176  }
1177 
1178  // 3d features
1179  if(_preferencesDialog->isFeaturesShown(1) && !odom.info().localMap.empty())
1180  {
1181  if(!lost)
1182  {
1183  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1184  cloud->resize(odom.info().localMap.size());
1185  int i=0;
1186  for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
1187  {
1188  // filter very far features from current location
1189  if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
1190  {
1191  (*cloud)[i].x = iter->second.x;
1192  (*cloud)[i].y = iter->second.y;
1193  (*cloud)[i].z = iter->second.z;
1194 
1195  // green = inlier, yellow = outliers
1196  bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
1197  (*cloud)[i].r = inlier?0:255;
1198  (*cloud)[i].g = 255;
1199  (*cloud)[i].b = 0;
1200  if(!_preferencesDialog->isOdomOnlyInliersShown() || inlier)
1201  {
1202  ++i;
1203  }
1204  }
1205  }
1206  cloud->resize(i);
1207 
1208  _cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
1209  _cloudViewer->setCloudVisibility("featuresOdom", true);
1210  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
1211  }
1212  featuresUpdated = true;
1213  }
1214 
1215  if(_preferencesDialog->isFrustumsShown(1))
1216  {
1217  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
1218  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
1219  {
1220  std::list<std::string> splitted = uSplitNumChar(iter.key());
1221  if(splitted.size() == 2)
1222  {
1223  int id = std::atoi(splitted.back().c_str());
1224  if(splitted.front().compare("f_odom_") == 0 &&
1225  odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
1226  {
1227  _cloudViewer->removeFrustum(iter.key());
1228  }
1229  }
1230  }
1231 
1232  for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
1233  {
1234  std::string frustumId = uFormat("f_odom_%d", iter->first);
1235  if(_cloudViewer->getAddedFrustums().contains(frustumId))
1236  {
1237  _cloudViewer->updateFrustumPose(frustumId, _odometryCorrection*iter->second);
1238  }
1239  else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
1240  {
1241  const CameraModel & model = odom.info().localBundleModels.at(iter->first);
1242  Transform t = model.localTransform();
1243  if(!t.isNull())
1244  {
1245  QColor color = Qt::yellow;
1246  _cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color);
1247  }
1248  }
1249  }
1250  }
1251  }
1252  if(!dataIgnored)
1253  {
1254  if(!cloudUpdated && _cloudViewer->getAddedClouds().contains("cloudOdom"))
1255  {
1256  _cloudViewer->setCloudVisibility("cloudOdom", false);
1257  }
1258  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanOdom"))
1259  {
1260  _cloudViewer->setCloudVisibility("scanOdom", false);
1261  }
1262  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanMapOdom"))
1263  {
1264  _cloudViewer->setCloudVisibility("scanMapOdom", false);
1265  }
1266  if(!featuresUpdated && _cloudViewer->getAddedClouds().contains("featuresOdom"))
1267  {
1268  _cloudViewer->setCloudVisibility("featuresOdom", false);
1269  }
1270  }
1271  }
1272 
1273  if(!odom.pose().isNull())
1274  {
1275  _odometryReceived = true;
1276  // update camera position
1277  _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
1278 
1279  if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
1280  {
1281  _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
1282  }
1283  else if(!odom.data().stereoCameraModel().localTransform().isNull())
1284  {
1285  _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
1286  }
1287 
1288  }
1289  _cloudViewer->update();
1290 
1291  if(_ui->graphicsView_graphView->isVisible())
1292  {
1293  if(!pose.isNull() && !odom.pose().isNull())
1294  {
1295  _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
1296  _ui->graphicsView_graphView->update();
1297  }
1298  }
1299 
1300  if(_ui->dockWidget_odometry->isVisible() &&
1301  !odom.data().imageRaw().empty())
1302  {
1303  if(_ui->imageView_odometry->isFeaturesShown())
1304  {
1305  if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
1306  {
1307  if(_preferencesDialog->isOdomOnlyInliersShown())
1308  {
1309  std::multimap<int, cv::KeyPoint> kpInliers;
1310  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1311  {
1312  kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
1313  }
1314  _ui->imageView_odometry->setFeatures(
1315  kpInliers,
1316  odom.data().depthRaw(),
1317  Qt::green);
1318  }
1319  else
1320  {
1321  _ui->imageView_odometry->setFeatures(
1322  odom.info().words,
1323  odom.data().depthRaw(),
1324  Qt::yellow);
1325  }
1326  }
1327  else if(odom.info().type == (int)Odometry::kTypeF2F ||
1328  odom.info().type == (int)Odometry::kTypeViso2 ||
1329  odom.info().type == (int)Odometry::kTypeFovis ||
1330  odom.info().type == (int)Odometry::kTypeMSCKF)
1331  {
1332  std::vector<cv::KeyPoint> kpts;
1333  cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
1334  _ui->imageView_odometry->setFeatures(
1335  kpts,
1336  odom.data().depthRaw(),
1337  Qt::red);
1338  }
1339  }
1340 
1341  //detect if it is OdometryMono initialization
1342  bool monoInitialization = false;
1343  if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F)
1344  {
1345  monoInitialization = true;
1346  }
1347 
1348  _ui->imageView_odometry->clearLines();
1349  if(lost && !monoInitialization)
1350  {
1351  if(lostStateChanged)
1352  {
1353  // save state
1354  _odomImageShow = _ui->imageView_odometry->isImageShown();
1355  _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
1356  }
1357  _ui->imageView_odometry->setImageDepth(odom.data().imageRaw());
1358  _ui->imageView_odometry->setImageShown(true);
1359  _ui->imageView_odometry->setImageDepthShown(true);
1360  }
1361  else
1362  {
1363  if(lostStateChanged)
1364  {
1365  // restore state
1366  _ui->imageView_odometry->setImageShown(_odomImageShow);
1367  _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
1368  }
1369 
1370  _ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
1371  if(_ui->imageView_odometry->isImageDepthShown() && !odom.data().depthOrRightRaw().empty())
1372  {
1373  _ui->imageView_odometry->setImageDepth(odom.data().depthOrRightRaw());
1374  }
1375 
1376  if( odom.info().type == (int)Odometry::kTypeF2M ||
1377  odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
1378  odom.info().type == (int)Odometry::kTypeMSCKF)
1379  {
1380  if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
1381  {
1382  for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
1383  {
1384  _ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
1385  }
1386  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1387  {
1388  _ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
1389  }
1390  }
1391  }
1392  if((odom.info().type == (int)Odometry::kTypeF2F ||
1393  odom.info().type == (int)Odometry::kTypeViso2 ||
1394  odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size())
1395  {
1396  if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
1397  {
1398  //draw lines
1399  UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
1400  std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
1401  for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
1402  {
1403  if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
1404  {
1405  _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
1406  }
1407  if(_ui->imageView_odometry->isLinesShown())
1408  {
1409  _ui->imageView_odometry->addLine(
1410  odom.info().newCorners[i].x,
1411  odom.info().newCorners[i].y,
1412  odom.info().refCorners[i].x,
1413  odom.info().refCorners[i].y,
1414  inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
1415  }
1416  }
1417  }
1418  }
1419  }
1420  if(!odom.data().imageRaw().empty())
1421  {
1422  _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
1423  }
1424 
1425  _ui->imageView_odometry->update();
1426  }
1427 
1428  if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
1429  {
1430  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
1431  }
1432 
1433  //Process info
1434  _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
1435  _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());
1436  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
1437  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
1438  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
1439  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
1440  _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
1441  _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());
1442  _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());
1443  _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());
1444  _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());
1445  _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());
1446  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1447  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1448  _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
1449  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
1450  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
1451  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
1452  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
1453  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
1454  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1455  _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());
1456  _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
1457 
1458  float x=0.0f,y,z, roll,pitch,yaw;
1459  if(!odom.info().transform.isNull())
1460  {
1461  odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1462  _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1463  _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1464  _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1465  _ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1466  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1467  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1468  }
1469 
1470  if(!odom.info().transformFiltered.isNull())
1471  {
1472  odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1473  _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1474  _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1475  _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1476  _ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1477  _ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1478  _ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1479  }
1480  if(odom.info().interval > 0)
1481  {
1482  _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
1483  _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1484  }
1485 
1486  if(!odom.info().transformGroundTruth.isNull())
1487  {
1488  odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1489  _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1490  _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1491  _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1492  _ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1493  _ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1494  _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1495  if(odom.info().interval > 0)
1496  {
1497  _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1498  }
1499  }
1500 
1501  //cumulative pose
1502  if(!odom.pose().isNull())
1503  {
1504  odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1505  _ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1506  _ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1507  _ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1508  _ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1509  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1510  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1511  }
1512  if(!odom.data().groundTruth().isNull())
1513  {
1514  odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1515  _ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1516  _ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1517  _ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1518  _ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1519  _ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1520  _ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1521  }
1522 
1523  if(odom.info().distanceTravelled > 0)
1524  {
1525  _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
1526  }
1527 
1528  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
1529  _processingOdometry = false;
1530 
1531  Q_EMIT(odometryProcessed());
1532 }
1533 
1534 void MainWindow::processStats(const rtabmap::Statistics & stat)
1535 {
1536  _processingStatistics = true;
1537  ULOGGER_DEBUG("");
1538  QTime time, totalTime;
1539  time.start();
1540  totalTime.start();
1541  //Affichage des stats et images
1542 
1543  if(_firstStamp == 0.0)
1544  {
1545  _firstStamp = stat.stamp();
1546  }
1547 
1548  int refMapId = -1, loopMapId = -1;
1549  if(uContains(stat.getSignatures(), stat.refImageId()))
1550  {
1551  refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
1552  }
1553  int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
1554  int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
1555  if(_cachedSignatures.contains(loopId))
1556  {
1557  loopMapId = _cachedSignatures.value(loopId).mapId();
1558  }
1559 
1560  _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
1561 
1562  if(stat.extended())
1563  {
1564  float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
1565  if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
1566  {
1567  UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
1568  }
1569 
1570  UDEBUG("");
1571  bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
1572 
1573  bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
1574 
1575  bool fastMovement = (bool)uValue(stat.data(), Statistics::kMemoryFast_movement(), 0.0f);
1576 
1577  // update cache
1578  Signature signature;
1579  if(uContains(stat.getSignatures(), stat.refImageId()))
1580  {
1581  signature = stat.getSignatures().at(stat.refImageId());
1582  signature.sensorData().uncompressData(); // make sure data are uncompressed
1583 
1584  if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
1585  signature.getWeight()>=0) // ignore intermediate nodes for the cache
1586  {
1587  if(smallMovement || fastMovement)
1588  {
1589  _cachedSignatures.insert(-1, signature); // negative means temporary
1590  }
1591  else
1592  {
1593  _cachedSignatures.insert(signature.id(), signature);
1594  _cachedMemoryUsage += signature.sensorData().getMemoryUsed();
1595  unsigned int count = 0;
1596  for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().upper_bound(-1); jter!=signature.getWords3().end(); ++jter)
1597  {
1598  if(util3d::isFinite(jter->second))
1599  {
1600  ++count;
1601  }
1602  }
1603  _cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
1604  }
1605  }
1606  }
1607 
1608  // For intermediate empty nodes, keep latest image shown
1609  if(signature.getWeight() >= 0 &&
1610  (!signature.sensorData().imageRaw().empty() || signature.getWords().size()))
1611  {
1612  _ui->imageView_source->clear();
1613  _ui->imageView_loopClosure->clear();
1614 
1615  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
1616  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
1617 
1618  _ui->label_matchId->clear();
1619 
1620 
1621  int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1622  bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
1623  int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
1624  bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
1625  _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
1626 
1627  if(rehearsalMerged > 0)
1628  {
1629  _ui->imageView_source->setBackgroundColor(Qt::blue);
1630  }
1631  else if(proximityTimeDetections > 0)
1632  {
1633  _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
1634  }
1635  else if(scanMatchingSuccess)
1636  {
1637  _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
1638  }
1639  else if(rehearsedSimilarity)
1640  {
1641  _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
1642  }
1643  else if(smallMovement)
1644  {
1645  _ui->imageView_source->setBackgroundColor(Qt::gray);
1646  }
1647  else if(fastMovement)
1648  {
1649  _ui->imageView_source->setBackgroundColor(Qt::magenta);
1650  }
1651  // Set color code as tooltip
1652  if(_ui->label_refId->toolTip().isEmpty())
1653  {
1654  _ui->label_refId->setToolTip(
1655  "Background Color Code:\n"
1656  " Blue = Weight Update Merged\n"
1657  " Dark Blue = Weight Update\n"
1658  " Dark Yellow = Proximity Detection in Time\n"
1659  " Dark Cyan = Neighbor Link Refined\n"
1660  " Gray = Small Movement\n"
1661  " Magenta = Fast Movement\n"
1662  "Feature Color code:\n"
1663  " Green = New\n"
1664  " Yellow = New but Not Unique\n"
1665  " Red = In Vocabulary\n"
1666  " Blue = In Vocabulary and in Previous Signature\n"
1667  " Pink = In Vocabulary and in Loop Closure Signature\n"
1668  " Gray = Not Quantized to Vocabulary");
1669  }
1670  // Set color code as tooltip
1671  if(_ui->label_matchId->toolTip().isEmpty())
1672  {
1673  _ui->label_matchId->setToolTip(
1674  "Background Color Code:\n"
1675  " Green = Accepted Loop Closure Detection\n"
1676  " Red = Rejected Loop Closure Detection\n"
1677  " Yellow = Proximity Detection in Space\n"
1678  "Feature Color code:\n"
1679  " Red = In Vocabulary\n"
1680  " Pink = In Vocabulary and in Loop Closure Signature\n"
1681  " Gray = Not Quantized to Vocabulary");
1682  }
1683 
1684  UDEBUG("time= %d ms", time.restart());
1685 
1686  int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
1687  float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
1688  int matchId = 0;
1689  Signature loopSignature;
1690  int shownLoopId = 0;
1691  if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
1692  {
1693  bool show = true;
1694  if(stat.loopClosureId() > 0)
1695  {
1696  _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
1697  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
1698  if(highestHypothesisIsSaved)
1699  {
1700  _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
1701  }
1702  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
1703  matchId = stat.loopClosureId();
1704  }
1705  else if(stat.proximityDetectionId())
1706  {
1707  _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
1708  _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
1709  matchId = stat.proximityDetectionId();
1710  }
1711  else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
1712  {
1713  show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
1714  if(show)
1715  {
1716  _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
1717  _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
1718  _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
1719  }
1720  }
1721  else
1722  {
1723  show = _preferencesDialog->imageHighestHypShown();
1724  if(show)
1725  {
1726  _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
1727  }
1728  }
1729 
1730  if(show)
1731  {
1732  shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
1733  QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
1734  if(iter != _cachedSignatures.end())
1735  {
1736  // uncompress after copy to avoid keeping uncompressed data in memory
1737  loopSignature = iter.value();
1738  loopSignature.sensorData().uncompressData();
1739  }
1740  }
1741  }
1742  _refIds.push_back(stat.refImageId());
1743  _loopClosureIds.push_back(matchId);
1744  if(matchId > 0)
1745  {
1746  _cachedLocalizationsCount[matchId] += 1.0f;
1747  }
1748 
1749  //update image views
1750  {
1751  UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
1752  UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
1753  qimageThread.start();
1754  qimageLoopThread.start();
1755  qimageThread.join();
1756  qimageLoopThread.join();
1757  QImage img = qimageThread.getQImage();
1758  QImage lcImg = qimageLoopThread.getQImage();
1759  UDEBUG("time= %d ms", time.restart());
1760 
1761  if(!img.isNull())
1762  {
1763  _ui->imageView_source->setImage(img);
1764  }
1765  if(!signature.sensorData().depthOrRightRaw().empty())
1766  {
1767  _ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
1768  }
1769  if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
1770  {
1771  QRect sceneRect;
1772  if(signature.sensorData().cameraModels().size())
1773  {
1774  for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
1775  {
1776  sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
1777  sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
1778  }
1779  }
1780  else if(signature.sensorData().stereoCameraModel().isValidForProjection())
1781  {
1782  sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
1783  }
1784  if(sceneRect.isValid())
1785  {
1786  _ui->imageView_source->setSceneRect(sceneRect);
1787  }
1788  }
1789  if(!lcImg.isNull())
1790  {
1791  _ui->imageView_loopClosure->setImage(lcImg);
1792  }
1793  if(!loopSignature.sensorData().depthOrRightRaw().empty())
1794  {
1795  _ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
1796  }
1797  if(_ui->imageView_loopClosure->sceneRect().isNull())
1798  {
1799  _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
1800  }
1801  }
1802 
1803  UDEBUG("time= %d ms", time.restart());
1804 
1805  // do it after scaling
1806  this->drawKeypoints(signature.getWords(), loopSignature.getWords());
1807 
1808  UDEBUG("time= %d ms", time.restart());
1809 
1810  _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
1811  _ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
1812 
1813  // loop closure view
1814  if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
1815  !stat.loopClosureTransform().isNull() &&
1816  !loopSignature.sensorData().imageRaw().empty())
1817  {
1818  // the last loop closure data
1819  Transform loopClosureTransform = stat.loopClosureTransform();
1820  signature.setPose(loopClosureTransform);
1821  _loopClosureViewer->setData(loopSignature, signature);
1822  if(_ui->dockWidget_loopClosureViewer->isVisible())
1823  {
1824  UTimer loopTimer;
1825  _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
1826  UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
1827  _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
1828  }
1829 
1830  UDEBUG("time= %d ms", time.restart());
1831  }
1832  }
1833 
1834  // PDF AND LIKELIHOOD
1835  if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
1836  {
1837  UDEBUG("");
1838  _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
1839 
1840  ULOGGER_DEBUG("");
1841  //Adjust thresholds
1842  float value;
1843  value = float(_preferencesDialog->getLoopThr());
1844  Q_EMIT(loopClosureThrChanged(value));
1845  }
1846  if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
1847  {
1848  _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
1849  }
1850  if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
1851  {
1852  _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
1853  }
1854 
1855  // Update statistics tool box
1856  const std::map<std::string, float> & statistics = stat.data();
1857  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
1858  {
1859  //ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
1860  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
1861  }
1862 
1863  UDEBUG("time= %d ms", time.restart());
1864 
1865  //======================
1866  // RGB-D Mapping stuff
1867  //======================
1868  // update clouds
1869  if(stat.poses().size())
1870  {
1871  // update pose only if odometry is not received
1872  std::map<int, int> mapIds;
1873  std::map<int, Transform> groundTruth;
1874  std::map<int, std::string> labels;
1875  for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
1876  {
1877  mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
1878  if(!iter->second.getGroundTruthPose().isNull())
1879  {
1880  groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
1881  }
1882  if(!iter->second.getLabel().empty())
1883  {
1884  labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
1885  }
1886  }
1887 
1888  std::map<int, Transform> poses = stat.poses();
1889  UDEBUG("time= %d ms", time.restart());
1890 
1891  if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
1892  {
1893  _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
1894 
1895  std::map<int, Signature>::const_iterator iter = stat.getSignatures().find(poses.rbegin()->first);
1896  if(iter != stat.getSignatures().end())
1897  {
1898  if(iter->second.sensorData().cameraModels().size() && !iter->second.sensorData().cameraModels()[0].localTransform().isNull())
1899  {
1900  _cloudViewer->updateCameraFrustums(poses.rbegin()->second, iter->second.sensorData().cameraModels());
1901  }
1902  else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull())
1903  {
1904  _cloudViewer->updateCameraFrustum(poses.rbegin()->second, iter->second.sensorData().stereoCameraModel());
1905  }
1906  }
1907 
1908  if(_ui->graphicsView_graphView->isVisible())
1909  {
1910  _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
1911  }
1912  }
1913 
1914  if(_cachedSignatures.contains(-1))
1915  {
1916  if(poses.find(stat.refImageId())!=poses.end())
1917  {
1918  poses.insert(std::make_pair(-1, poses.at(stat.refImageId())));
1919  poses.erase(stat.refImageId());
1920  }
1921  if(groundTruth.find(stat.refImageId())!=groundTruth.end())
1922  {
1923  groundTruth.insert(std::make_pair(-1, groundTruth.at(stat.refImageId())));
1924  groundTruth.erase(stat.refImageId());
1925  }
1926  }
1927 
1928  std::map<std::string, float> updateCloudSats;
1929  updateMapCloud(
1930  poses,
1931  stat.constraints(),
1932  mapIds,
1933  labels,
1934  groundTruth,
1935  false,
1936  &updateCloudSats);
1937 
1938  _odometryReceived = false;
1939 
1940  UDEBUG("time= %d ms", time.restart());
1941 
1942  for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
1943  {
1944  _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
1945  }
1946  }
1947  _odometryCorrection = stat.mapCorrection();
1948 
1949  if( _ui->graphicsView_graphView->isVisible())
1950  {
1951  // update posterior on the graph view
1952  if(_preferencesDialog->isPosteriorGraphView() &&
1953  stat.posterior().size())
1954  {
1955  _ui->graphicsView_graphView->updatePosterior(stat.posterior());
1956  }
1957  else if(_preferencesDialog->isRGBDMode())
1958  {
1959  if(_preferencesDialog->isWordsCountGraphView() &&
1960  _cachedWordsCount.size())
1961  {
1962  _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
1963  }
1964  else if(_preferencesDialog->isLocalizationsCountGraphView() &&
1965  _cachedLocalizationsCount.size())
1966  {
1967  _ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
1968  }
1969  }
1970  // update local path on the graph view
1971  _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
1972  if(stat.localPath().size() == 0)
1973  {
1974  // clear the global path if set (goal reached)
1975  _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
1976  }
1977  // update current goal id
1978  if(stat.currentGoalId() > 0)
1979  {
1980  _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
1981  }
1982  }
1983  UDEBUG("");
1984 
1985  _cachedSignatures.remove(-1); // remove tmp negative ids
1986 
1987  // keep only compressed data in cache
1988  if(_cachedSignatures.contains(stat.refImageId()))
1989  {
1990  Signature & s = *_cachedSignatures.find(stat.refImageId());
1991  _cachedMemoryUsage -= s.sensorData().getMemoryUsed();
1992  s.sensorData().setImageRaw(cv::Mat());
1993  s.sensorData().setDepthOrRightRaw(cv::Mat());
1994  s.sensorData().setUserDataRaw(cv::Mat());
1995  s.sensorData().setLaserScanRaw(
1996  LaserScan(
1997  cv::Mat(),
1998  signature.sensorData().laserScanRaw().maxPoints(),
1999  signature.sensorData().laserScanRaw().maxRange(),
2000  signature.sensorData().laserScanRaw().format(),
2001  signature.sensorData().laserScanRaw().localTransform()));
2002  s.sensorData().clearOccupancyGridRaw();
2003  _cachedMemoryUsage += s.sensorData().getMemoryUsed();
2004  }
2005 
2006  UDEBUG("");
2007  }
2008  else if(!stat.extended() && stat.loopClosureId()>0)
2009  {
2010  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
2011  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
2012  }
2013  else
2014  {
2015  _ui->label_matchId->clear();
2016  }
2017  float elapsedTime = static_cast<float>(totalTime.elapsed());
2018  UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
2019  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
2020  if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
2021  {
2022  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
2023  }
2024 
2025  if(!_preferencesDialog->isImagesKept())
2026  {
2027  _cachedSignatures.clear();
2028  _cachedMemoryUsage = 0;
2029  _cachedWordsCount.clear();
2030  }
2031  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2032  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2033 #ifdef RTABMAP_OCTOMAP
2034  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2035 #endif
2036  if(_state != kMonitoring && _state != kDetecting)
2037  {
2038  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2039  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2040  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2041  }
2042 
2043  _processingStatistics = false;
2044 
2045  Q_EMIT(statsProcessed());
2046 }
2047 
2048 void MainWindow::updateMapCloud(
2049  const std::map<int, Transform> & posesIn,
2050  const std::multimap<int, Link> & constraints,
2051  const std::map<int, int> & mapIdsIn,
2052  const std::map<int, std::string> & labels,
2053  const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
2054  bool verboseProgress,
2055  std::map<std::string, float> * stats)
2056 {
2057  UTimer timer;
2058  UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
2059  (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
2060  if(posesIn.size())
2061  {
2062  _currentPosesMap = posesIn;
2063  _currentPosesMap.erase(-1); // don't keep -1 if it is there
2064  _currentLinksMap = constraints;
2065  _currentMapIds = mapIdsIn;
2066  _currentLabels = labels;
2067  _currentGTPosesMap = groundTruths;
2068  _currentGTPosesMap.erase(-1);
2069  if(_state != kMonitoring && _state != kDetecting)
2070  {
2071  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
2072  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
2073  }
2074  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
2075  }
2076 
2077  // filter duplicated poses
2078  std::map<int, Transform> poses;
2079  std::map<int, int> mapIds;
2080  if(_preferencesDialog->isCloudFiltering() && posesIn.size())
2081  {
2082  float radius = _preferencesDialog->getCloudFilteringRadius();
2083  float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
2084  bool hasNeg = posesIn.find(-1) != posesIn.end();
2085  if(hasNeg)
2086  {
2087  std::map<int, Transform> posesInTmp = posesIn;
2088  posesInTmp.erase(-1);
2089  poses = rtabmap::graph::radiusPosesFiltering(posesInTmp, radius, angle);
2090  }
2091  else
2092  {
2093  poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
2094  }
2095  for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
2096  {
2097  std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
2098  if(jter!=mapIdsIn.end())
2099  {
2100  mapIds.insert(*jter);
2101  }
2102  else
2103  {
2104  UERROR("map id of node %d not found!", iter->first);
2105  }
2106  }
2107  //keep -1
2108  if(hasNeg)
2109  {
2110  poses.insert(*posesIn.find(-1));
2111  }
2112 
2113  if(verboseProgress)
2114  {
2115  _progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(posesIn.size()));
2116  QApplication::processEvents();
2117  }
2118  }
2119  else
2120  {
2121  poses = posesIn;
2122  mapIds = mapIdsIn;
2123  }
2124 
2125  std::map<int, bool> posesMask;
2126  for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
2127  {
2128  posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
2129  }
2130  _ui->widget_mapVisibility->setMap(posesIn, posesMask);
2131 
2132  if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
2133  {
2134  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2135  {
2136  std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
2137  if(gtIter!=groundTruths.end())
2138  {
2139  iter->second = gtIter->second;
2140  }
2141  else
2142  {
2143  UWARN("Not found ground truth pose for node %d", iter->first);
2144  }
2145  }
2146  }
2147  else if(_currentGTPosesMap.size() == 0)
2148  {
2149  _ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
2150  }
2151 
2152  int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
2153  if(maxNodes > 0 && poses.size()>1)
2154  {
2155  std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
2156  std::map<int, Transform> nearestPoses;
2157  nearestPoses.insert(*poses.rbegin());
2158  for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2159  {
2160  std::map<int, Transform>::iterator pter = poses.find(*iter);
2161  if(pter != poses.end())
2162  {
2163  nearestPoses.insert(*pter);
2164  }
2165  }
2166  //add negative...
2167  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2168  {
2169  if(iter->first > 0)
2170  {
2171  break;
2172  }
2173  nearestPoses.insert(*iter);
2174  }
2175  poses=nearestPoses;
2176  }
2177 
2178  // Map updated! regenerate the assembled cloud, last pose is the new one
2179  UDEBUG("Update map with %d locations", poses.size());
2180  QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
2181  int i=1;
2182  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2183  {
2184  if(!iter->second.isNull())
2185  {
2186  std::string cloudName = uFormat("cloud%d", iter->first);
2187 
2188  if(iter->first < 0)
2189  {
2190  viewerClouds.remove(cloudName);
2191  _cloudViewer->removeCloud(cloudName);
2192  }
2193 
2194  // 3d point cloud
2195  bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
2196  if(update3dCloud)
2197  {
2198  // update cloud
2199  if(viewerClouds.contains(cloudName))
2200  {
2201  // Update only if the pose has changed
2202  Transform tCloud;
2203  _cloudViewer->getPose(cloudName, tCloud);
2204  if(tCloud.isNull() || iter->second != tCloud)
2205  {
2206  if(!_cloudViewer->updateCloudPose(cloudName, iter->second))
2207  {
2208  UERROR("Updating pose cloud %d failed!", iter->first);
2209  }
2210  }
2211  _cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
2212  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
2213  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
2214  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
2215  }
2216  else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
2217  _cachedClouds.find(iter->first) == _cachedClouds.end() &&
2218  _cachedSignatures.contains(iter->first))
2219  {
2220  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2221  if(_cloudViewer->getAddedClouds().contains(cloudName))
2222  {
2223  _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
2224  }
2225  }
2226  }
2227  else if(viewerClouds.contains(cloudName))
2228  {
2229  _cloudViewer->setCloudVisibility(cloudName.c_str(), false);
2230  }
2231 
2232  // 2d point cloud
2233  std::string scanName = uFormat("scan%d", iter->first);
2234  if(iter->first < 0)
2235  {
2236  viewerClouds.remove(scanName);
2237  _cloudViewer->removeCloud(scanName);
2238  }
2239  if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
2240  {
2241  if(viewerClouds.contains(scanName))
2242  {
2243  // Update only if the pose has changed
2244  Transform tScan;
2245  _cloudViewer->getPose(scanName, tScan);
2246  if(tScan.isNull() || iter->second != tScan)
2247  {
2248  if(!_cloudViewer->updateCloudPose(scanName, iter->second))
2249  {
2250  UERROR("Updating pose scan %d failed!", iter->first);
2251  }
2252  }
2253  _cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
2254  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
2255  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
2256  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
2257  }
2258  else if(_cachedSignatures.contains(iter->first))
2259  {
2260  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2261  if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
2262  {
2263  this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2264  }
2265  }
2266  }
2267  else if(viewerClouds.contains(scanName))
2268  {
2269  _cloudViewer->setCloudVisibility(scanName.c_str(), false);
2270  }
2271 
2272  // occupancy grids
2273  bool updateGridMap =
2274  ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
2275  (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
2276  _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
2277  bool updateOctomap = false;
2278 #ifdef RTABMAP_OCTOMAP
2279  updateOctomap =
2280  _cloudViewer->isVisible() &&
2281  _preferencesDialog->isOctomapUpdated() &&
2282  _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
2283 #endif
2284  if(updateGridMap || updateOctomap)
2285  {
2286  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2287  if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
2288  {
2289  cv::Mat ground;
2290  cv::Mat obstacles;
2291  cv::Mat empty;
2292 
2293  jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
2294 
2295  _occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
2296 
2297 #ifdef RTABMAP_OCTOMAP
2298  if(updateOctomap)
2299  {
2300  if((ground.empty() || ground.channels() > 2) &&
2301  (obstacles.empty() || obstacles.channels() > 2))
2302  {
2303  cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
2304  _octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
2305  }
2306  else if(!ground.empty() || !obstacles.empty())
2307  {
2308  UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
2309  }
2310  }
2311 #endif
2312  }
2313  }
2314 
2315  // 3d features
2316  std::string featuresName = uFormat("features%d", iter->first);
2317  if(iter->first < 0)
2318  {
2319  viewerClouds.remove(featuresName);
2320  _cloudViewer->removeCloud(featuresName);
2321  }
2322  if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
2323  {
2324  if(viewerClouds.contains(featuresName))
2325  {
2326  // Update only if the pose has changed
2327  Transform tFeatures;
2328  _cloudViewer->getPose(featuresName, tFeatures);
2329  if(tFeatures.isNull() || iter->second != tFeatures)
2330  {
2331  if(!_cloudViewer->updateCloudPose(featuresName, iter->second))
2332  {
2333  UERROR("Updating pose features %d failed!", iter->first);
2334  }
2335  }
2336  _cloudViewer->setCloudVisibility(featuresName, _preferencesDialog->isFeaturesShown(0));
2337  _cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
2338  }
2339  else if(_cachedSignatures.contains(iter->first))
2340  {
2341  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2342  if(!jter->getWords3().empty())
2343  {
2344  this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2345  }
2346  }
2347  }
2348  else if(viewerClouds.contains(featuresName))
2349  {
2350  _cloudViewer->setCloudVisibility(featuresName.c_str(), false);
2351  }
2352 
2353  if(verboseProgress)
2354  {
2355  _progressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
2356  _progressDialog->incrementStep();
2357  if(poses.size() < 200 || i % 100 == 0)
2358  {
2359  QApplication::processEvents();
2360  if(_progressCanceled)
2361  {
2362  break;
2363  }
2364  }
2365  }
2366  }
2367 
2368  ++i;
2369  }
2370 
2371  //remove not used clouds
2372  for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
2373  {
2374  std::list<std::string> splitted = uSplitNumChar(iter.key());
2375  int id = 0;
2376  if(splitted.size() == 2)
2377  {
2378  id = std::atoi(splitted.back().c_str());
2379  if(splitted.front().at(splitted.front().size()-1) == '-')
2380  {
2381  id*=-1;
2382  }
2383  }
2384 
2385  if(id != 0 && poses.find(id) == poses.end())
2386  {
2387  if(_cloudViewer->getCloudVisibility(iter.key()))
2388  {
2389  UDEBUG("Hide %s", iter.key().c_str());
2390  _cloudViewer->setCloudVisibility(iter.key(), false);
2391  }
2392  }
2393  }
2394 
2395  UDEBUG("");
2396  if(stats)
2397  {
2398  stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f));
2399  }
2400 
2401  // update 3D graphes (show all poses)
2402  _cloudViewer->removeAllGraphs();
2403  _cloudViewer->removeCloud("graph_nodes");
2404  if(!_preferencesDialog->isFrustumsShown(0))
2405  {
2406  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
2407  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2408  {
2409  std::list<std::string> splitted = uSplitNumChar(iter.key());
2410  if(splitted.size() == 2)
2411  {
2412  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0))
2413  {
2414  _cloudViewer->removeFrustum(iter.key());
2415  }
2416  }
2417  }
2418  }
2419 
2420  Transform mapToGt = Transform::getIdentity();
2421  if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
2422  {
2423  mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
2424  }
2425 
2426  if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
2427  {
2428  UTimer timerGraph;
2429  // Find all graphs
2430  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
2431  for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
2432  {
2433  int mapId = uValue(_currentMapIds, iter->first, -1);
2434 
2435  if(_preferencesDialog->isGraphsShown())
2436  {
2437  //edges
2438  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2439  if(kter == graphs.end())
2440  {
2441  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
2442  }
2443  pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
2444  kter->second->push_back(pt);
2445  }
2446 
2447  // get local transforms for frustums on the graph
2448  if(_preferencesDialog->isFrustumsShown(0))
2449  {
2450  std::string frustumId = uFormat("f_%d", iter->first);
2451  if(_cloudViewer->getAddedFrustums().contains(frustumId))
2452  {
2453  _cloudViewer->updateFrustumPose(frustumId, iter->second);
2454  }
2455  else if(_cachedSignatures.contains(iter->first))
2456  {
2457  const Signature & s = _cachedSignatures.value(iter->first);
2458  // Supporting only one frustum per node
2459  if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
2460  {
2461  Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
2462  if(!t.isNull())
2463  {
2464  QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2465  _cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
2466 
2467  if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
2468  {
2469  std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
2470  color = Qt::gray;
2471  _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color);
2472  }
2473  }
2474  }
2475  }
2476  }
2477  }
2478 
2479  //Ground truth graph?
2480  for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
2481  {
2482  int mapId = -100;
2483 
2484  if(_preferencesDialog->isGraphsShown())
2485  {
2486  //edges
2487  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2488  if(kter == graphs.end())
2489  {
2490  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
2491  }
2492  Transform t = mapToGt*iter->second;
2493  pcl::PointXYZ pt(t.x(), t.y(), t.z());
2494  kter->second->push_back(pt);
2495  }
2496  }
2497 
2498  // add graphs
2499  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
2500  {
2501  QColor color = Qt::gray;
2502  if(iter->first >= 0)
2503  {
2504  color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
2505  }
2506  _cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
2507  }
2508 
2509  if(_preferencesDialog->isFrustumsShown(0))
2510  {
2511  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
2512  UDEBUG("remove not used frustums");
2513  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2514  {
2515  std::list<std::string> splitted = uSplitNumChar(iter.key());
2516  if(splitted.size() == 2)
2517  {
2518  int id = std::atoi(splitted.back().c_str());
2519  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
2520  _currentPosesMap.find(id) == _currentPosesMap.end())
2521  {
2522  _cloudViewer->removeFrustum(iter.key());
2523  }
2524  }
2525  }
2526  }
2527 
2528  UDEBUG("timerGraph=%fs", timerGraph.ticks());
2529  }
2530 
2531  UDEBUG("labels.size()=%d", (int)labels.size());
2532 
2533  // Update labels
2534  _cloudViewer->removeAllTexts();
2535  if(_preferencesDialog->isLabelsShown() && labels.size())
2536  {
2537  for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
2538  {
2539  if(posesIn.find(iter->first)!=posesIn.end())
2540  {
2541  int mapId = uValue(mapIdsIn, iter->first, -1);
2542  QColor color = Qt::gray;
2543  if(mapId >= 0)
2544  {
2545  color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2546  }
2547  _cloudViewer->addOrUpdateText(
2548  std::string("label_") + uNumber2Str(iter->first),
2549  iter->second,
2550  _currentPosesMap.at(iter->first),
2551  0.1,
2552  color);
2553  }
2554  }
2555  }
2556 
2557  UDEBUG("");
2558  if(stats)
2559  {
2560  stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f));
2561  }
2562 
2563 #ifdef RTABMAP_OCTOMAP
2564  _cloudViewer->removeOctomap();
2565  _cloudViewer->removeCloud("octomap_cloud");
2566  if(_preferencesDialog->isOctomapUpdated())
2567  {
2568  UDEBUG("");
2569  UTimer time;
2570  _octomap->update(poses);
2571  UINFO("Octomap update time = %fs", time.ticks());
2572  }
2573  if(stats)
2574  {
2575  stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f));
2576  }
2577  if(_preferencesDialog->isOctomapShown())
2578  {
2579  UDEBUG("");
2580  UTimer time;
2581  if(_preferencesDialog->getOctomapRenderingType() > 0)
2582  {
2583  _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
2584  }
2585  else
2586  {
2587  pcl::IndicesPtr obstacles(new std::vector<int>);
2588  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = _octomap->createCloud(_preferencesDialog->getOctomapTreeDepth(), obstacles.get());
2589  if(obstacles->size())
2590  {
2591  _cloudViewer->addCloud("octomap_cloud", cloud);
2592  _cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize());
2593  }
2594  }
2595  UINFO("Octomap show 3d map time = %fs", time.ticks());
2596  }
2597  UDEBUG("");
2598  if(stats)
2599  {
2600  stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f));
2601  }
2602 #endif
2603 
2604  // Update occupancy grid map in 3D map view and graph view
2605  if(_ui->graphicsView_graphView->isVisible())
2606  {
2607  _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
2608  if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
2609  {
2610  std::map<int, Transform> gtPoses = _currentGTPosesMap;
2611  for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
2612  {
2613  iter->second = mapToGt * iter->second;
2614  }
2615  _ui->graphicsView_graphView->updateGTGraph(gtPoses);
2616  }
2617  else
2618  {
2619  _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
2620  }
2621  }
2622  cv::Mat map8U;
2623  if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
2624  {
2625  float xMin, yMin;
2626  float resolution = _occupancyGrid->getCellSize();
2627  cv::Mat map8S;
2628 #ifdef RTABMAP_OCTOMAP
2629  if(_preferencesDialog->isOctomap2dGrid())
2630  {
2631  map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
2632 
2633  }
2634  else
2635 #endif
2636  {
2637  if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0)
2638  {
2639  _occupancyGrid->update(poses);
2640  }
2641  if(stats)
2642  {
2643  stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
2644  }
2645  map8S = _occupancyGrid->getMap(xMin, yMin);
2646  }
2647  if(!map8S.empty())
2648  {
2649  //convert to gray scaled map
2650  map8U = util3d::convertMap2Image8U(map8S);
2651 
2652  if(_preferencesDialog->getGridMapShown())
2653  {
2654  float opacity = _preferencesDialog->getGridMapOpacity();
2655  _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
2656  }
2657  if(_ui->graphicsView_graphView->isVisible())
2658  {
2659  _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
2660  }
2661  }
2662  }
2663  _ui->graphicsView_graphView->update();
2664 
2665  UDEBUG("");
2666  if(stats)
2667  {
2668  stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f));
2669  }
2670 
2671  if(!_preferencesDialog->getGridMapShown())
2672  {
2673  UDEBUG("");
2674  _cloudViewer->removeOccupancyGridMap();
2675  }
2676 
2677  if(viewerClouds.contains("cloudOdom"))
2678  {
2679  if(!_preferencesDialog->isCloudsShown(1))
2680  {
2681  UDEBUG("");
2682  _cloudViewer->setCloudVisibility("cloudOdom", false);
2683  }
2684  else
2685  {
2686  UDEBUG("");
2687  _cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
2688  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
2689  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
2690  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
2691  }
2692  }
2693  if(viewerClouds.contains("scanOdom"))
2694  {
2695  if(!_preferencesDialog->isScansShown(1))
2696  {
2697  UDEBUG("");
2698  _cloudViewer->setCloudVisibility("scanOdom", false);
2699  }
2700  else
2701  {
2702  UDEBUG("");
2703  _cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
2704  _cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
2705  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
2706  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
2707  }
2708  }
2709  if(viewerClouds.contains("scanMapOdom"))
2710  {
2711  if(!_preferencesDialog->isScansShown(1))
2712  {
2713  UDEBUG("");
2714  _cloudViewer->setCloudVisibility("scanMapOdom", false);
2715  }
2716  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
2717  {
2718  UDEBUG("");
2719  _cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
2720  _cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
2721  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
2722  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
2723  }
2724  }
2725  if(viewerClouds.contains("featuresOdom"))
2726  {
2727  if(!_preferencesDialog->isFeaturesShown(1))
2728  {
2729  UDEBUG("");
2730  _cloudViewer->setCloudVisibility("featuresOdom", false);
2731  }
2732  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
2733  {
2734  UDEBUG("");
2735  _cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
2736  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
2737  }
2738  }
2739 
2740  // activate actions
2741  if(_state != kMonitoring && _state != kDetecting)
2742  {
2743  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
2744  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
2745  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
2746 #ifdef RTABMAP_OCTOMAP
2747  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
2748 #else
2749  _ui->actionExport_octomap->setEnabled(false);
2750 #endif
2751  }
2752 
2753  UDEBUG("");
2754  _cloudViewer->update();
2755  UDEBUG("");
2756 }
2757 
2758 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
2759 {
2760  UASSERT(!pose.isNull());
2761  std::string cloudName = uFormat("cloud%d", nodeId);
2762  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
2763  if(_cloudViewer->getAddedClouds().contains(cloudName))
2764  {
2765  UERROR("Cloud %d already added to map.", nodeId);
2766  return outputPair;
2767  }
2768 
2769  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
2770  if(iter == _cachedSignatures.end())
2771  {
2772  UERROR("Node %d is not in the cache.", nodeId);
2773  return outputPair;
2774  }
2775 
2776  UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
2777 
2778  if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
2779  (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
2780  {
2781  cv::Mat image, depth;
2782  SensorData data = iter->sensorData();
2783  data.uncompressData(&image, &depth, 0);
2784  ParametersMap allParameters = _preferencesDialog->getAllParameters();
2785  bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
2786  bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
2787  Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
2788  Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
2789  if(rectifyOnlyFeatures && !imagesAlreadyRectified)
2790  {
2791  if(data.cameraModels().size())
2792  {
2793  UTimer time;
2794  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
2795  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
2796  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
2797  cv::Mat rectifiedImages = data.imageRaw().clone();
2798  bool initRectMaps = _rectCameraModels.empty();
2799  if(initRectMaps)
2800  {
2801  _rectCameraModels.resize(data.cameraModels().size());
2802  }
2803  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
2804  {
2805  if(data.cameraModels()[i].isValidForRectification())
2806  {
2807  if(initRectMaps)
2808  {
2809  _rectCameraModels[i] = data.cameraModels()[i];
2810  if(!_rectCameraModels[i].isRectificationMapInitialized())
2811  {
2812  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
2813  _rectCameraModels[i].initRectificationMap();
2814  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
2815  }
2816  }
2817  UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
2818  _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
2819  cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
2820  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
2821  }
2822  else
2823  {
2824  UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
2825  i, data.id(),
2826  data.cameraModels()[i].imageWidth(),
2827  data.cameraModels()[i].imageHeight());
2828  }
2829  }
2830  UINFO("Time rectification: %fs", time.ticks());
2831  data.setImageRaw(rectifiedImages);
2832  image = rectifiedImages;
2833  }
2834  }
2835 
2836  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
2837  pcl::IndicesPtr indices(new std::vector<int>);
2838  UASSERT_MSG(nodeId == -1 || nodeId == data.id(), uFormat("nodeId=%d data.id()=%d", nodeId, data.id()).c_str());
2839 
2840  // Create organized cloud
2841  cloud = util3d::cloudRGBFromSensorData(data,
2842  _preferencesDialog->getCloudDecimation(0),
2843  _preferencesDialog->getCloudMaxDepth(0),
2844  _preferencesDialog->getCloudMinDepth(0),
2845  indices.get(),
2846  allParameters,
2847  _preferencesDialog->getCloudRoiRatios(0));
2848 
2849  // view point
2850  Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
2851  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
2852  {
2853  viewPoint[0] = data.cameraModels()[0].localTransform().x();
2854  viewPoint[1] = data.cameraModels()[0].localTransform().y();
2855  viewPoint[2] = data.cameraModels()[0].localTransform().z();
2856  }
2857  else if(!data.stereoCameraModel().localTransform().isNull())
2858  {
2859  viewPoint[0] = data.stereoCameraModel().localTransform().x();
2860  viewPoint[1] = data.stereoCameraModel().localTransform().y();
2861  viewPoint[2] = data.stereoCameraModel().localTransform().z();
2862  }
2863 
2864  // filtering pipeline
2865  if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
2866  {
2867  cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
2868  //generate indices for all points (they are all valid)
2869  indices->resize(cloud->size());
2870  for(unsigned int i=0; i<cloud->size(); ++i)
2871  {
2872  indices->at(i) = i;
2873  }
2874  }
2875 
2876  // Do ceiling/floor filtering
2877  if(indices->size() &&
2878  (_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
2879  _preferencesDialog->getCeilingFilteringHeight() != 0.0))
2880  {
2881  // perform in /map frame
2882  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
2883  indices = rtabmap::util3d::passThrough(
2884  cloudTransformed,
2885  indices,
2886  "z",
2887  _preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
2888  _preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
2889  }
2890 
2891  // Do radius filtering after voxel filtering ( a lot faster)
2892  if(indices->size() &&
2893  _preferencesDialog->getNoiseRadius() > 0.0 &&
2894  _preferencesDialog->getNoiseMinNeighbors() > 0)
2895  {
2896  indices = rtabmap::util3d::radiusFiltering(
2897  cloud,
2898  indices,
2899  _preferencesDialog->getNoiseRadius(),
2900  _preferencesDialog->getNoiseMinNeighbors());
2901  }
2902 
2903  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2904  if(_preferencesDialog->isSubtractFiltering() &&
2905  _preferencesDialog->getSubtractFilteringRadius() > 0.0 &&
2906  nodeId > 0)
2907  {
2908  pcl::IndicesPtr beforeFiltering = indices;
2909  if( cloud->size() &&
2910  _previousCloud.first>0 &&
2911  _previousCloud.second.first.first.get() != 0 &&
2912  _previousCloud.second.second.get() != 0 &&
2913  _previousCloud.second.second->size() &&
2914  _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
2915  {
2916  UTimer time;
2917 
2918  rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
2919 
2920  //UWARN("saved new.pcd and old.pcd");
2921  //pcl::io::savePCDFile("new.pcd", *cloud, *indices);
2922  //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
2923 
2924  if(_preferencesDialog->isSubtractFiltering())
2925  {
2926  if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
2927  {
2928  //normals required
2929  if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
2930  {
2931  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
2932  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
2933  }
2934  else
2935  {
2936  UWARN("Cloud subtraction with angle filtering is activated but "
2937  "cloud normal K search is 0. Subtraction is done with angle.");
2938  }
2939  }
2940 
2941  if(cloudWithNormals->size() &&
2942  _previousCloud.second.first.second.get() &&
2943  _previousCloud.second.first.second->size())
2944  {
2945  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t);
2946  indices = rtabmap::util3d::subtractFiltering(
2947  cloudWithNormals,
2948  indices,
2949  previousCloud,
2950  _previousCloud.second.second,
2951  _preferencesDialog->getSubtractFilteringRadius(),
2952  _preferencesDialog->getSubtractFilteringAngle(),
2953  _preferencesDialog->getSubtractFilteringMinPts());
2954  }
2955  else
2956  {
2957  pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t);
2958  indices = rtabmap::util3d::subtractFiltering(
2959  cloud,
2960  indices,
2961  previousCloud,
2962  _previousCloud.second.second,
2963  _preferencesDialog->getSubtractFilteringRadius(),
2964  _preferencesDialog->getSubtractFilteringMinPts());
2965  }
2966 
2967 
2968  UINFO("Time subtract filtering %d from %d -> %d (%fs)",
2969  (int)_previousCloud.second.second->size(),
2970  (int)beforeFiltering->size(),
2971  (int)indices->size(),
2972  time.ticks());
2973  }
2974  }
2975  // keep all indices for next subtraction
2976  _previousCloud.first = nodeId;
2977  _previousCloud.second.first.first = cloud;
2978  _previousCloud.second.first.second = cloudWithNormals;
2979  _previousCloud.second.second = beforeFiltering;
2980  }
2981 
2982  if(indices->size())
2983  {
2984  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
2985  bool added = false;
2986  if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
2987  {
2988  // Fast organized mesh
2989  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
2990  output = util3d::extractIndices(cloud, indices, false, true);
2991  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
2992  output,
2993  _preferencesDialog->getCloudMeshingAngle(),
2994  _preferencesDialog->isCloudMeshingQuad(),
2995  _preferencesDialog->getCloudMeshingTriangleSize(),
2996  viewPoint);
2997  if(polygons.size())
2998  {
2999  // remove unused vertices to save memory
3000  pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
3001  std::vector<pcl::Vertices> outputPolygons;
3002  std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
3003 
3004  if(_preferencesDialog->isCloudMeshingTexture() && !image.empty())
3005  {
3006  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
3007  pcl::toPCLPointCloud2(*outputFiltered, textureMesh->cloud);
3008  textureMesh->tex_polygons.push_back(outputPolygons);
3009  int w = cloud->width;
3010  int h = cloud->height;
3011  UASSERT(w > 1 && h > 1);
3012  textureMesh->tex_coordinates.resize(1);
3013  int nPoints = (int)outputFiltered->size();
3014  textureMesh->tex_coordinates[0].resize(nPoints);
3015  for(int i=0; i<nPoints; ++i)
3016  {
3017  //uv
3018  UASSERT(i < (int)denseToOrganizedIndices.size());
3019  int originalVertex = denseToOrganizedIndices[i];
3020  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
3021  float(originalVertex % w) / float(w), // u
3022  float(h - originalVertex / w) / float(h)); // v
3023  }
3024 
3025  pcl::TexMaterial mesh_material;
3026  mesh_material.tex_d = 1.0f;
3027  mesh_material.tex_Ns = 75.0f;
3028  mesh_material.tex_illum = 1;
3029 
3030  std::stringstream tex_name;
3031  tex_name << "material_" << nodeId;
3032  tex_name >> mesh_material.tex_name;
3033 
3034  mesh_material.tex_file = "";
3035 
3036  textureMesh->tex_materials.push_back(mesh_material);
3037 
3038  if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, image, pose))
3039  {
3040  UERROR("Adding texture mesh %d to viewer failed!", nodeId);
3041  }
3042  else
3043  {
3044  added = true;
3045  }
3046  }
3047  else if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
3048  {
3049  UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
3050  }
3051  else
3052  {
3053  added = true;
3054  }
3055  }
3056  }
3057  else
3058  {
3059  if(_preferencesDialog->isCloudMeshing())
3060  {
3061  UWARN("Online meshing is activated but the generated cloud is "
3062  "dense (voxel filtering is used or multiple cameras are used). Disable "
3063  "online meshing in Preferences->3D Rendering to hide this warning.");
3064  }
3065 
3066  if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
3067  {
3068  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
3069  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3070  }
3071 
3072  QColor color = Qt::gray;
3073  if(mapId >= 0)
3074  {
3075  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3076  }
3077 
3078  output = util3d::extractIndices(cloud, indices, false, true);
3079 
3080  if(cloudWithNormals->size())
3081  {
3082  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
3083  outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false);
3084 
3085  if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color))
3086  {
3087  UERROR("Adding cloud %d to viewer failed!", nodeId);
3088  }
3089  else
3090  {
3091  added = true;
3092  }
3093  }
3094  else
3095  {
3096  if(!_cloudViewer->addCloud(cloudName, output, pose, color))
3097  {
3098  UERROR("Adding cloud %d to viewer failed!", nodeId);
3099  }
3100  else
3101  {
3102  added = true;
3103  }
3104  }
3105  }
3106  if(added)
3107  {
3108  outputPair.first = output;
3109  outputPair.second = indices;
3110  if(_preferencesDialog->isCloudsKept() && nodeId > 0)
3111  {
3112  _cachedClouds.insert(std::make_pair(nodeId, outputPair));
3113  _createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
3114  }
3115  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
3116  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
3117  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
3118  }
3119  else if(nodeId>0)
3120  {
3121  _cachedEmptyClouds.insert(nodeId);
3122  }
3123  }
3124  else if(nodeId>0)
3125  {
3126  _cachedEmptyClouds.insert(nodeId);
3127  }
3128  }
3129 
3130  return outputPair;
3131 }
3132 
3133 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
3134 {
3135  std::string scanName = uFormat("scan%d", nodeId);
3136  if(_cloudViewer->getAddedClouds().contains(scanName))
3137  {
3138  UERROR("Scan %d already added to map.", nodeId);
3139  return;
3140  }
3141 
3142  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3143  if(iter == _cachedSignatures.end())
3144  {
3145  UERROR("Node %d is not in the cache.", nodeId);
3146  return;
3147  }
3148 
3149  if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
3150  {
3151  LaserScan scan;
3152  iter->sensorData().uncompressData(0, 0, &scan);
3153 
3154  if(_preferencesDialog->getDownsamplingStepScan(0) > 1 ||
3155  _preferencesDialog->getScanMaxRange(0) > 0.0f ||
3156  _preferencesDialog->getScanMinRange(0) > 0.0f)
3157  {
3158  scan = util3d::commonFiltering(scan,
3159  _preferencesDialog->getDownsamplingStepScan(0),
3160  _preferencesDialog->getScanMinRange(0),
3161  _preferencesDialog->getScanMaxRange(0));
3162  }
3163 
3164  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
3165  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
3166  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
3167  pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
3168  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
3169  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
3170  if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3171  {
3172  cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
3173  }
3174  else if(scan.hasNormals() && scan.hasIntensity() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3175  {
3176  cloudIWithNormals = util3d::laserScanToPointCloudINormal(scan, scan.localTransform());
3177  }
3178  else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3179  {
3180  cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
3181  }
3182  else if(scan.hasRGB())
3183  {
3184  cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
3185  }
3186  else if(scan.hasIntensity())
3187  {
3188  cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
3189  }
3190  else
3191  {
3192  cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
3193  }
3194 
3195  if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
3196  {
3197  if(cloud.get())
3198  {
3199  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
3200  }
3201  if(cloudRGB.get())
3202  {
3203  cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
3204  }
3205  if(cloudI.get())
3206  {
3207  cloudI = util3d::voxelize(cloudI, _preferencesDialog->getCloudVoxelSizeScan(0));
3208  }
3209  }
3210 
3211  // Do ceiling/floor filtering
3212  if((!scan.is2d()) && // don't filter 2D scans
3213  (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
3214  _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
3215  {
3216  if(cloudRGBWithNormals.get())
3217  {
3218  // perform in /map frame
3219  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
3220  cloudTransformed = rtabmap::util3d::passThrough(
3221  cloudTransformed,
3222  "z",
3223  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3224  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3225 
3226  //transform back in sensor frame
3227  cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3228  }
3229  if(cloudIWithNormals.get())
3230  {
3231  // perform in /map frame
3232  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudIWithNormals, pose);
3233  cloudTransformed = rtabmap::util3d::passThrough(
3234  cloudTransformed,
3235  "z",
3236  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3237  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3238 
3239  //transform back in sensor frame
3240  cloudIWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3241  }
3242  if(cloudWithNormals.get())
3243  {
3244  // perform in /map frame
3245  pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
3246  cloudTransformed = rtabmap::util3d::passThrough(
3247  cloudTransformed,
3248  "z",
3249  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3250  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3251 
3252  //transform back in sensor frame
3253  cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3254  }
3255  if(cloudRGB.get())
3256  {
3257  // perform in /map frame
3258  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
3259  cloudTransformed = rtabmap::util3d::passThrough(
3260  cloudTransformed,
3261  "z",
3262  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3263  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3264 
3265  //transform back in sensor frame
3266  cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3267  }
3268  if(cloudI.get())
3269  {
3270  // perform in /map frame
3271  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudTransformed = util3d::transformPointCloud(cloudI, pose);
3272  cloudTransformed = rtabmap::util3d::passThrough(
3273  cloudTransformed,
3274  "z",
3275  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3276  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3277 
3278  //transform back in sensor frame
3279  cloudI = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3280  }
3281  if(cloud.get())
3282  {
3283  // perform in /map frame
3284  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
3285  cloudTransformed = rtabmap::util3d::passThrough(
3286  cloudTransformed,
3287  "z",
3288  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3289  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3290 
3291  //transform back in sensor frame
3292  cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3293  }
3294  }
3295 
3296  if( (cloud.get() || cloudRGB.get() || cloudI.get()) &&
3297  (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
3298  {
3299  Eigen::Vector3f scanViewpoint(
3300  scan.localTransform().x(),
3301  scan.localTransform().y(),
3302  scan.localTransform().z());
3303 
3304  pcl::PointCloud<pcl::Normal>::Ptr normals;
3305  if(cloud.get() && cloud->size())
3306  {
3307  if(scan.is2d())
3308  {
3309  normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3310  }
3311  else
3312  {
3313  normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3314  }
3315  cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
3316  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3317  cloud.reset();
3318  }
3319  else if(cloudRGB.get() && cloudRGB->size())
3320  {
3321  // Assuming 3D
3322  normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3323  cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3324  pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
3325  cloudRGB.reset();
3326  }
3327  else if(cloudI.get())
3328  {
3329  if(scan.is2d())
3330  {
3331  normals = util3d::computeFastOrganizedNormals2D(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3332  }
3333  else
3334  {
3335  normals = util3d::computeNormals(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3336  }
3337  cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
3338  pcl::concatenateFields(*cloud, *normals, *cloudIWithNormals);
3339  cloudI.reset();
3340  }
3341  }
3342 
3343  QColor color = Qt::gray;
3344  if(mapId >= 0)
3345  {
3346  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3347  }
3348  bool added = false;
3349  if(cloudRGBWithNormals.get())
3350  {
3351  added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
3352  if(added && nodeId > 0)
3353  {
3354  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
3355  }
3356  }
3357  else if(cloudIWithNormals.get())
3358  {
3359  added = _cloudViewer->addCloud(scanName, cloudIWithNormals, pose, color);
3360  if(added && nodeId > 0)
3361  {
3362  if(scan.is2d())
3363  {
3364  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
3365  }
3366  else
3367  {
3368  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
3369  }
3370  }
3371  }
3372  else if(cloudWithNormals.get())
3373  {
3374  added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
3375  if(added && nodeId > 0)
3376  {
3377  if(scan.is2d())
3378  {
3379  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
3380  }
3381  else
3382  {
3383  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
3384  }
3385  }
3386  }
3387  else if(cloudRGB.get())
3388  {
3389  added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
3390  if(added && nodeId > 0)
3391  {
3392  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
3393  }
3394  }
3395  else if(cloudI.get())
3396  {
3397  added = _cloudViewer->addCloud(scanName, cloudI, pose, color);
3398  if(added && nodeId > 0)
3399  {
3400  if(scan.is2d())
3401  {
3402  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYI, scan.localTransform());
3403  }
3404  else
3405  {
3406  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
3407  }
3408  }
3409  }
3410  else
3411  {
3412  UASSERT(cloud.get());
3413  added = _cloudViewer->addCloud(scanName, cloud, pose, color);
3414  if(added && nodeId > 0)
3415  {
3416  if(scan.is2d())
3417  {
3418  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXY, scan.localTransform());
3419  }
3420  else
3421  {
3422  scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
3423  }
3424  }
3425  }
3426  if(!added)
3427  {
3428  UERROR("Adding cloud %d to viewer failed!", nodeId);
3429  }
3430  else
3431  {
3432  if(nodeId > 0)
3433  {
3434  _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in scan frame
3435  }
3436 
3437  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(0));
3438  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
3439  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
3440  }
3441  }
3442 }
3443 
3444 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
3445 {
3446  UDEBUG("");
3447  UASSERT(!pose.isNull());
3448  std::string cloudName = uFormat("features%d", nodeId);
3449  if(_cloudViewer->getAddedClouds().contains(cloudName))
3450  {
3451  UERROR("Features cloud %d already added to map.", nodeId);
3452  return;
3453  }
3454 
3455  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3456  if(iter == _cachedSignatures.end())
3457  {
3458  UERROR("Node %d is not in the cache.", nodeId);
3459  return;
3460  }
3461 
3462  if(_createdFeatures.find(nodeId) != _createdFeatures.end())
3463  {
3464  UDEBUG("Features cloud %d already created.");
3465  return;
3466  }
3467 
3468  if(iter->getWords3().size())
3469  {
3470  UINFO("Create cloud from 3D words");
3471  QColor color = Qt::gray;
3472  if(mapId >= 0)
3473  {
3474  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3475  }
3476 
3477  cv::Mat rgb;
3478  if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
3479  {
3480  SensorData data = iter->sensorData();
3481  data.uncompressData(&rgb, 0, 0);
3482  }
3483 
3484  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3485  cloud->resize(iter->getWords3().size());
3486  int oi=0;
3487  UASSERT(iter->getWords().size() == iter->getWords3().size());
3488  std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
3489  float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
3490  UDEBUG("rgb.channels()=%d");
3491  for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
3492  jter!=iter->getWords3().end(); ++jter, ++kter)
3493  {
3494  if(util3d::isFinite(jter->second) && (maxDepth == 0.0f || jter->second.z < maxDepth))
3495  {
3496  (*cloud)[oi].x = jter->second.x;
3497  (*cloud)[oi].y = jter->second.y;
3498  (*cloud)[oi].z = jter->second.z;
3499  int u = kter->second.pt.x+0.5;
3500  int v = kter->second.pt.y+0.5;
3501  if(!rgb.empty() &&
3502  uIsInBounds(u, 0, rgb.cols-1) &&
3503  uIsInBounds(v, 0, rgb.rows-1))
3504  {
3505  if(rgb.channels() == 1)
3506  {
3507  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
3508  }
3509  else
3510  {
3511  cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
3512  (*cloud)[oi].b = bgr.val[0];
3513  (*cloud)[oi].g = bgr.val[1];
3514  (*cloud)[oi].r = bgr.val[2];
3515  }
3516  }
3517  else
3518  {
3519  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
3520  }
3521  ++oi;
3522  }
3523  }
3524  cloud->resize(oi);
3525  if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
3526  {
3527  UERROR("Adding features cloud %d to viewer failed!", nodeId);
3528  }
3529  else if(nodeId > 0)
3530  {
3531  _createdFeatures.insert(std::make_pair(nodeId, cloud));
3532  }
3533  }
3534  else
3535  {
3536  return;
3537  }
3538 
3539  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
3540  UDEBUG("");
3541 }
3542 
3543 Transform MainWindow::alignPosesToGroundTruth(
3544  const std::map<int, Transform> & poses,
3545  const std::map<int, Transform> & groundTruth)
3546 {
3547  Transform t = Transform::getIdentity();
3548  if(groundTruth.size() && poses.size())
3549  {
3550  float translational_rmse = 0.0f;
3551  float translational_mean = 0.0f;
3552  float translational_median = 0.0f;
3553  float translational_std = 0.0f;
3554  float translational_min = 0.0f;
3555  float translational_max = 0.0f;
3556  float rotational_rmse = 0.0f;
3557  float rotational_mean = 0.0f;
3558  float rotational_median = 0.0f;
3559  float rotational_std = 0.0f;
3560  float rotational_min = 0.0f;
3561  float rotational_max = 0.0f;
3562 
3563  t = graph::calcRMSE(
3564  groundTruth,
3565  poses,
3566  translational_rmse,
3567  translational_mean,
3568  translational_median,
3569  translational_std,
3570  translational_min,
3571  translational_max,
3572  rotational_rmse,
3573  rotational_mean,
3574  rotational_median,
3575  rotational_std,
3576  rotational_min,
3577  rotational_max);
3578 
3579  // ground truth live statistics
3580  UINFO("translational_rmse=%f", translational_rmse);
3581  UINFO("translational_mean=%f", translational_mean);
3582  UINFO("translational_median=%f", translational_median);
3583  UINFO("translational_std=%f", translational_std);
3584  UINFO("translational_min=%f", translational_min);
3585  UINFO("translational_max=%f", translational_max);
3586 
3587  UINFO("rotational_rmse=%f", rotational_rmse);
3588  UINFO("rotational_mean=%f", rotational_mean);
3589  UINFO("rotational_median=%f", rotational_median);
3590  UINFO("rotational_std=%f", rotational_std);
3591  UINFO("rotational_min=%f", rotational_min);
3592  UINFO("rotational_max=%f", rotational_max);
3593  }
3594  return t;
3595 }
3596 
3597 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
3598 {
3599  UINFO("Update visibility %d", nodeId);
3600  QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
3601  Transform pose;
3602  if(_currentGTPosesMap.size() &&
3603  _ui->actionAnchor_clouds_to_ground_truth->isChecked() &&
3604  _currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end())
3605  {
3606  pose = _currentGTPosesMap.at(nodeId);
3607  }
3608  else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
3609  {
3610  pose = _currentPosesMap.at(nodeId);
3611  }
3612 
3613  if(!pose.isNull() || !visible)
3614  {
3615  if(_preferencesDialog->isCloudsShown(0))
3616  {
3617  std::string cloudName = uFormat("cloud%d", nodeId);
3618  if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
3619  {
3620  createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
3621  }
3622  else if(viewerClouds.contains(cloudName))
3623  {
3624  if(visible)
3625  {
3626  //make sure the transformation was done
3627  _cloudViewer->updateCloudPose(cloudName, pose);
3628  }
3629  _cloudViewer->setCloudVisibility(cloudName, visible);
3630  }
3631  }
3632 
3633  if(_preferencesDialog->isScansShown(0))
3634  {
3635  std::string scanName = uFormat("scan%d", nodeId);
3636  if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
3637  {
3638  createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
3639  }
3640  else if(viewerClouds.contains(scanName))
3641  {
3642  if(visible)
3643  {
3644  //make sure the transformation was done
3645  _cloudViewer->updateCloudPose(scanName, pose);
3646  }
3647  _cloudViewer->setCloudVisibility(scanName, visible);
3648  }
3649  }
3650 
3651  _cloudViewer->update();
3652  }
3653 }
3654 
3655 void MainWindow::updateGraphView()
3656 {
3657  if(_ui->dockWidget_graphViewer->isVisible())
3658  {
3659  UDEBUG("Graph visible!");
3660  if(_currentPosesMap.size())
3661  {
3662  this->updateMapCloud(
3663  std::map<int, Transform>(_currentPosesMap),
3664  std::multimap<int, Link>(_currentLinksMap),
3665  std::map<int, int>(_currentMapIds),
3666  std::map<int, std::string>(_currentLabels),
3667  std::map<int, Transform>(_currentGTPosesMap));
3668  }
3669  }
3670 }
3671 
3672 void MainWindow::processRtabmapEventInit(int status, const QString & info)
3673 {
3674  if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
3675  {
3676  _progressDialog->resetProgress();
3677  _progressDialog->show();
3678  this->changeState(MainWindow::kInitializing);
3679  }
3680  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
3681  {
3682  _progressDialog->setValue(_progressDialog->maximumSteps());
3683  this->changeState(MainWindow::kInitialized);
3684 
3685  if(!_openedDatabasePath.isEmpty())
3686  {
3687  this->downloadAllClouds();
3688  }
3689  }
3690  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
3691  {
3692  _progressDialog->resetProgress();
3693  _progressDialog->show();
3694  if(_state!=kApplicationClosing)
3695  {
3696  this->changeState(MainWindow::kClosing);
3697  }
3698  }
3699  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
3700  {
3701  _progressDialog->setValue(_progressDialog->maximumSteps());
3702 
3703  if(_databaseUpdated)
3704  {
3705  if(!_newDatabasePath.isEmpty())
3706  {
3707  if(!_newDatabasePathOutput.isEmpty())
3708  {
3709  bool removed = true;
3710  if(QFile::exists(_newDatabasePathOutput))
3711  {
3712  removed = QFile::remove(_newDatabasePathOutput);
3713  }
3714  if(removed)
3715  {
3716  if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
3717  {
3718  std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
3719  UINFO(msg.c_str());
3720  QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
3721  }
3722  else
3723  {
3724  std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".",
3725  _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
3726  UERROR(msg.c_str());
3727  QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
3728  }
3729  }
3730  else
3731  {
3732  std::string msg = uFormat("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
3733  _newDatabasePathOutput.toStdString().c_str(), _newDatabasePath.toStdString().c_str());
3734  UERROR(msg.c_str());
3735  QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
3736  }
3737  }
3738  else if(QFile::remove(_newDatabasePath))
3739  {
3740  UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
3741  }
3742  else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
3743  {
3744  UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
3745  }
3746 
3747  }
3748  else if(!_openedDatabasePath.isEmpty())
3749  {
3750  std::string msg = uFormat("Database \"%s\" updated.", _openedDatabasePath.toStdString().c_str());
3751  UINFO(msg.c_str());
3752  QMessageBox::information(this, tr("Database updated!"), QString(msg.c_str()));
3753  }
3754  }
3755  else if(!_newDatabasePath.isEmpty())
3756  {
3757  // just remove temporary database;
3758  if(QFile::remove(_newDatabasePath))
3759  {
3760  UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
3761  }
3762  else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
3763  {
3764  UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
3765  }
3766  }
3767  _openedDatabasePath.clear();
3768  _newDatabasePath.clear();
3769  _newDatabasePathOutput.clear();
3770  bool applicationClosing = _state == kApplicationClosing;
3771  this->changeState(MainWindow::kIdle);
3772  if(applicationClosing)
3773  {
3774  this->close();
3775  }
3776  }
3777  else
3778  {
3779  _progressDialog->incrementStep();
3780  QString msg(info);
3781  if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
3782  {
3783  _openedDatabasePath.clear();
3784  _newDatabasePath.clear();
3785  _newDatabasePathOutput.clear();
3786  _progressDialog->setAutoClose(false);
3787  msg.prepend(tr("[ERROR] "));
3788  _progressDialog->appendText(msg);
3789  this->changeState(MainWindow::kIdle);
3790  }
3791  else
3792  {
3793  _progressDialog->appendText(msg);
3794  }
3795  }
3796 }
3797 
3798 void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event)
3799 {
3800  _progressDialog->appendText("Downloading the map... done.");
3801  _progressDialog->incrementStep();
3802 
3803  if(event.getCode())
3804  {
3805  UERROR("Map received with code error %d!", event.getCode());
3806  _progressDialog->appendText(uFormat("[ERROR] Map received with code error %d!", event.getCode()).c_str());
3807  _progressDialog->setAutoClose(false);
3808  }
3809  else
3810  {
3811 
3812  _processingDownloadedMap = true;
3813  UINFO("Received map!");
3814  _progressDialog->appendText(tr(" poses = %1").arg(event.getPoses().size()));
3815  _progressDialog->appendText(tr(" constraints = %1").arg(event.getConstraints().size()));
3816 
3817  _progressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
3818  _progressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
3819  QApplication::processEvents();
3820 
3821  int addedSignatures = 0;
3822  std::map<int, int> mapIds;
3823  std::map<int, Transform> groundTruth;
3824  std::map<int, std::string> labels;
3825  for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
3826  iter!=event.getSignatures().end();
3827  ++iter)
3828  {
3829  mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
3830  if(!iter->second.getGroundTruthPose().isNull())
3831  {
3832  groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
3833  }
3834  if(!iter->second.getLabel().empty())
3835  {
3836  labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
3837  }
3838  if(!_cachedSignatures.contains(iter->first) ||
3839  (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty()))
3840  {
3841  _cachedSignatures.insert(iter->first, iter->second);
3842  _cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
3843  unsigned int count = 0;
3844  for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->second.getWords3().upper_bound(-1); jter!=iter->second.getWords3().end(); ++jter)
3845  {
3846  if(util3d::isFinite(jter->second))
3847  {
3848  ++count;
3849  }
3850  }
3851  _cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
3852  ++addedSignatures;
3853  }
3854  _progressDialog->incrementStep();
3855  QApplication::processEvents();
3856  }
3857  _progressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
3858  _progressDialog->incrementStep();
3859  QApplication::processEvents();
3860 
3861  _progressDialog->appendText("Inserting data in the cache... done.");
3862 
3863  if(event.getPoses().size())
3864  {
3865  _progressDialog->appendText("Updating the 3D map cloud...");
3866  _progressDialog->incrementStep();
3867  _progressDialog->setCancelButtonVisible(true);
3868  _progressCanceled = false;
3869  QApplication::processEvents();
3870  std::map<int, Transform> poses = event.getPoses();
3871  this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
3872 
3873  if( _ui->graphicsView_graphView->isVisible() &&
3874  _preferencesDialog->isWordsCountGraphView() &&
3875  _preferencesDialog->isRGBDMode()&&
3876  _cachedWordsCount.size())
3877  {
3878  _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
3879  }
3880 
3881  _progressDialog->appendText("Updating the 3D map cloud... done.");
3882  }
3883  else
3884  {
3885  _progressDialog->appendText("No poses received! The map cloud cannot be updated...");
3886  UINFO("Map received is empty! Cannot update the map cloud...");
3887  }
3888 
3889  _progressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
3890 
3891  if(!_preferencesDialog->isImagesKept())
3892  {
3893  _cachedSignatures.clear();
3894  _cachedMemoryUsage = 0;
3895  _cachedWordsCount.clear();
3896  }
3897  if(_state != kMonitoring && _state != kDetecting)
3898  {
3899  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
3900  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
3901  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
3902  }
3903  _processingDownloadedMap = false;
3904  }
3905  _progressDialog->setValue(_progressDialog->maximumSteps());
3906  _progressDialog->setCancelButtonVisible(false);
3907  _progressCanceled = false;
3908 
3909  Q_EMIT(rtabmapEvent3DMapProcessed());
3910 }
3911 
3912 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
3913 {
3914  if(!event.getPoses().empty())
3915  {
3916  _ui->graphicsView_graphView->setGlobalPath(event.getPoses());
3917  }
3918 
3919  _ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0), _preferencesDialog->isCacheSavedInFigures());
3920  _ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0), _preferencesDialog->isCacheSavedInFigures());
3921  _ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()), _preferencesDialog->isCacheSavedInFigures());
3922  _ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()), _preferencesDialog->isCacheSavedInFigures());
3923  _ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())), _preferencesDialog->isCacheSavedInFigures());
3924 
3925  if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
3926  {
3927  // use MessageBox
3928  if(event.getPoses().empty())
3929  {
3930  QMessageBox * warn = new QMessageBox(
3931  QMessageBox::Warning,
3932  tr("Setting goal failed!"),
3933  tr("Setting goal to location %1%2 failed. "
3934  "Some reasons: \n"
3935  "1) the robot is not yet localized in the map,\n"
3936  "2) the location doesn't exist in the map,\n"
3937  "3) the location is not linked to the global map or\n"
3938  "4) the location is too near of the current location (goal already reached).")
3939  .arg(event.getGoal())
3940  .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):""),
3941  QMessageBox::Ok,
3942  this);
3943  warn->setAttribute(Qt::WA_DeleteOnClose, true);
3944  warn->show();
3945  }
3946  else
3947  {
3948  QMessageBox * info = new QMessageBox(
3949  QMessageBox::Information,
3950  tr("Goal detected!"),
3951  tr("Global path computed to %1%2 (%3 poses, %4 m).")
3952  .arg(event.getGoal())
3953  .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):"")
3954  .arg(event.getPoses().size())
3955  .arg(graph::computePathLength(event.getPoses())),
3956  QMessageBox::Ok,
3957  this);
3958  info->setAttribute(Qt::WA_DeleteOnClose, true);
3959  info->show();
3960  }
3961  }
3962  else if(event.getPoses().empty() && _waypoints.size())
3963  {
3964  // resend the same goal
3965  uSleep(1000);
3966  this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
3967  }
3968 }
3969 
3970 void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
3971 {
3972  QMessageBox * warn = new QMessageBox(
3973  QMessageBox::Warning,
3974  tr("Setting label failed!"),
3975  tr("Setting label %1 to location %2 failed. "
3976  "Some reasons: \n"
3977  "1) the location doesn't exist in the map,\n"
3978  "2) the location has already a label.").arg(label).arg(id),
3979  QMessageBox::Ok,
3980  this);
3981  warn->setAttribute(Qt::WA_DeleteOnClose, true);
3982  warn->show();
3983 }
3984 
3985 void MainWindow::processRtabmapGoalStatusEvent(int status)
3986 {
3987  _ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
3988  if(_waypoints.size())
3989  {
3990  this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
3991  }
3992 }
3993 
3994 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
3995 {
3996  ULOGGER_DEBUG("");
3997  if(flags & PreferencesDialog::kPanelSource)
3998  {
3999  // Camera settings...
4000  _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
4001  this->updateSelectSourceMenu();
4002  _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
4003 
4004  if(_camera)
4005  {
4006  if(dynamic_cast<DBReader*>(_camera->camera()) != 0)
4007  {
4008  _camera->setImageRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
4009  }
4010  else
4011  {
4012  _camera->setImageRate(_preferencesDialog->getGeneralInputRate());
4013  }
4014  }
4015 
4016  }//This will update the statistics toolbox
4017 
4018  if(flags & PreferencesDialog::kPanelGeneral)
4019  {
4020  UDEBUG("General settings changed...");
4021  setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
4022  if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
4023  {
4024  _ui->graphicsView_graphView->clearPosterior();
4025  }
4026  }
4027 
4028  if(flags & PreferencesDialog::kPanelCloudRendering)
4029  {
4030  UDEBUG("Cloud rendering settings changed...");
4031  if(_currentPosesMap.size())
4032  {
4033  this->updateMapCloud(
4034  std::map<int, Transform>(_currentPosesMap),
4035  std::multimap<int, Link>(_currentLinksMap),
4036  std::map<int, int>(_currentMapIds),
4037  std::map<int, std::string>(_currentLabels),
4038  std::map<int, Transform>(_currentGTPosesMap));
4039  }
4040  }
4041 
4042  if(flags & PreferencesDialog::kPanelLogging)
4043  {
4044  UDEBUG("Logging settings changed...");
4045  ULogger::setLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerLevel());
4046  ULogger::setEventLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerEventLevel());
4047  ULogger::setType((ULogger::Type)_preferencesDialog->getGeneralLoggerType(),
4048  (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
4049  ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
4050  ULogger::setPrintThreadId(_preferencesDialog->getGeneralLoggerPrintThreadId());
4051  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
4052  }
4053 }
4054 
4055 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
4056 {
4057  applyPrefSettings(parameters, true); //post parameters
4058 }
4059 
4060 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
4061 {
4062  ULOGGER_DEBUG("");
4063  _occupancyGrid->parseParameters(parameters);
4064  if(parameters.size())
4065  {
4066  for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
4067  {
4068  UDEBUG("Parameter changed: Key=%s Value=%s", iter->first.c_str(), iter->second.c_str());
4069  }
4070 
4071  rtabmap::ParametersMap parametersModified = parameters;
4072 
4073  if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
4074  {
4075  _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4076  _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4077  _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4078  }
4079 
4080  if(_state != kIdle && parametersModified.size())
4081  {
4082  if(postParamEvent)
4083  {
4084  this->post(new ParamEvent(parametersModified));
4085  }
4086  }
4087 
4088  // update loop closure viewer parameters (Use Map parameters)
4089  _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
4090  _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
4091 
4092  // update graph view parameters
4093  if(uContains(parameters, Parameters::kRGBDLocalRadius()))
4094  {
4095  _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
4096  }
4097  }
4098 
4099  //update ui
4100  _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
4101  _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
4102  _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
4103 
4104  float value;
4105  value = float(_preferencesDialog->getLoopThr());
4106  Q_EMIT(loopClosureThrChanged(value));
4107 }
4108 
4109 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
4110 {
4111  UTimer timer;
4112 
4113  timer.start();
4114  ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
4115  if(refWords.size())
4116  {
4117  _ui->imageView_source->clearFeatures();
4118  }
4119  for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
4120  {
4121  int id = iter->first;
4122  QColor color;
4123  if(id<0)
4124  {
4125  // GRAY = NOT QUANTIZED
4126  color = Qt::gray;
4127  }
4128  else if(uContains(loopWords, id))
4129  {
4130  // PINK = FOUND IN LOOP SIGNATURE
4131  color = Qt::magenta;
4132  }
4133  else if(_lastIds.contains(id))
4134  {
4135  // BLUE = FOUND IN LAST SIGNATURE
4136  color = Qt::blue;
4137  }
4138  else if(id<=_lastId)
4139  {
4140  // RED = ALREADY EXISTS
4141  color = Qt::red;
4142  }
4143  else if(refWords.count(id) > 1)
4144  {
4145  // YELLOW = NEW and multiple times
4146  color = Qt::yellow;
4147  }
4148  else
4149  {
4150  // GREEN = NEW
4151  color = Qt::green;
4152  }
4153  _ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
4154  }
4155  ULOGGER_DEBUG("source time = %f s", timer.ticks());
4156 
4157  timer.start();
4158  ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
4159  QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
4160  if(loopWords.size())
4161  {
4162  _ui->imageView_loopClosure->clearFeatures();
4163  }
4164  for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
4165  {
4166  int id = iter->first;
4167  QColor color;
4168  if(id<0)
4169  {
4170  // GRAY = NOT QUANTIZED
4171  color = Qt::gray;
4172  }
4173  else if(uContains(refWords, id))
4174  {
4175  // PINK = FOUND IN LOOP SIGNATURE
4176  color = Qt::magenta;
4177  //To draw lines... get only unique correspondences
4178  if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
4179  {
4180  const cv::KeyPoint & a = refWords.find(id)->second;
4181  const cv::KeyPoint & b = iter->second;
4182  uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
4183  }
4184  }
4185  else if(id<=_lastId)
4186  {
4187  // RED = ALREADY EXISTS
4188  color = Qt::red;
4189  }
4190  else if(refWords.count(id) > 1)
4191  {
4192  // YELLOW = NEW and multiple times
4193  color = Qt::yellow;
4194  }
4195  else
4196  {
4197  // GREEN = NEW
4198  color = Qt::green;
4199  }
4200  _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
4201  }
4202 
4203  ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
4204 
4205  if(refWords.size()>0)
4206  {
4207  if((*refWords.rbegin()).first > _lastId)
4208  {
4209  _lastId = (*refWords.rbegin()).first;
4210  }
4211  _lastIds = QSet<int>::fromList(QList<int>::fromStdList(uKeysList(refWords)));
4212  }
4213 
4214  // Draw lines between corresponding features...
4215  float scaleSource = _ui->imageView_source->viewScale();
4216  float scaleLoop = _ui->imageView_loopClosure->viewScale();
4217  UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
4218  // Delta in actual window pixels
4219  float sourceMarginX = (_ui->imageView_source->width() - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
4220  float sourceMarginY = (_ui->imageView_source->height() - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
4221  float loopMarginX = (_ui->imageView_loopClosure->width() - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
4222  float loopMarginY = (_ui->imageView_loopClosure->height() - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
4223 
4224  float deltaX = 0;
4225  float deltaY = 0;
4226 
4227  if(_preferencesDialog->isVerticalLayoutUsed())
4228  {
4229  deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
4230  }
4231  else
4232  {
4233  deltaX = _ui->imageView_source->width();
4234  }
4235 
4236  if(refWords.size() && loopWords.size())
4237  {
4238  _ui->imageView_source->clearLines();
4239  _ui->imageView_loopClosure->clearLines();
4240  }
4241 
4242  for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
4243  iter!=uniqueCorrespondences.end();
4244  ++iter)
4245  {
4246 
4247  _ui->imageView_source->addLine(
4248  iter->first.x,
4249  iter->first.y,
4250  (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
4251  (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
4252  Qt::cyan);
4253 
4254  _ui->imageView_loopClosure->addLine(
4255  (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
4256  (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
4257  iter->second.x,
4258  iter->second.y,
4259  Qt::cyan);
4260  }
4261  _ui->imageView_source->update();
4262  _ui->imageView_loopClosure->update();
4263 }
4264 
4265 void MainWindow::showEvent(QShowEvent* anEvent)
4266 {
4267  //if the config file doesn't exist, make the GUI obsolete
4268  this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
4269 }
4270 
4271 void MainWindow::moveEvent(QMoveEvent* anEvent)
4272 {
4273  if(this->isVisible())
4274  {
4275  // HACK, there is a move event when the window is shown the first time.
4276  if(!_firstCall)
4277  {
4278  this->configGUIModified();
4279  }
4280  _firstCall = false;
4281  }
4282 }
4283 
4284 void MainWindow::resizeEvent(QResizeEvent* anEvent)
4285 {
4286  if(this->isVisible())
4287  {
4288  this->configGUIModified();
4289  }
4290 }
4291 
4292 void MainWindow::keyPressEvent(QKeyEvent *event)
4293 {
4294  //catch ctrl-s to save settings
4295  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
4296  {
4297  this->saveConfigGUI();
4298  }
4299 }
4300 
4301 bool MainWindow::eventFilter(QObject *obj, QEvent *event)
4302 {
4303  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
4304  {
4305  this->setWindowModified(true);
4306  }
4307  else if(event->type() == QEvent::FileOpen )
4308  {
4309  openDatabase(((QFileOpenEvent*)event)->file());
4310  }
4311  return QWidget::eventFilter(obj, event);
4312 }
4313 
4314 void MainWindow::updateSelectSourceMenu()
4315 {
4316  _ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice);
4317 
4318  _ui->actionMore_options->setChecked(
4319  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
4320  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
4321  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
4322  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
4323  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
4324  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages
4325  );
4326 
4327  _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
4328  _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
4329  _ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
4330  _ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
4331  _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
4332  _ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
4333  _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
4334  _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
4335  _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
4336  _ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
4337  _ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
4338  _ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
4339  _ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
4340  _ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
4341  _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
4342  _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
4343  _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
4344  _ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
4345 }
4346 
4347 void MainWindow::changeImgRateSetting()
4348 {
4349  Q_EMIT imgRateChanged(_ui->doubleSpinBox_stats_imgRate->value());
4350 }
4351 
4352 void MainWindow::changeDetectionRateSetting()
4353 {
4354  Q_EMIT detectionRateChanged(_ui->doubleSpinBox_stats_detectionRate->value());
4355 }
4356 
4357 void MainWindow::changeTimeLimitSetting()
4358 {
4359  Q_EMIT timeLimitChanged((float)_ui->doubleSpinBox_stats_timeLimit->value());
4360 }
4361 
4362 void MainWindow::changeMappingMode()
4363 {
4364  Q_EMIT mappingModeChanged(_ui->actionSLAM_mode->isChecked());
4365 }
4366 
4367 QString MainWindow::captureScreen(bool cacheInRAM, bool png)
4368 {
4369  QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (png?".png":".jpg"));
4370  _ui->statusbar->clearMessage();
4371  QPixmap figure = QPixmap::grabWidget(this);
4372 
4373  QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
4374  QString msg;
4375  if(cacheInRAM)
4376  {
4377  msg = tr("Screen captured \"%1\"").arg(name);
4378  QByteArray bytes;
4379  QBuffer buffer(&bytes);
4380  buffer.open(QIODevice::WriteOnly);
4381  figure.save(&buffer, png?"PNG":"JPEG");
4382  _autoScreenCaptureCachedImages.insert(name, bytes);
4383  }
4384  else
4385  {
4386  QDir dir;
4387  if(!dir.exists(targetDir))
4388  {
4389  dir.mkdir(targetDir);
4390  }
4391  targetDir += QDir::separator();
4392  targetDir += "Main_window";
4393  if(!dir.exists(targetDir))
4394  {
4395  dir.mkdir(targetDir);
4396  }
4397  targetDir += QDir::separator();
4398 
4399  figure.save(targetDir + name);
4400  msg = tr("Screen captured \"%1\"").arg(targetDir + name);
4401  }
4402  _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
4403  _ui->widget_console->appendMsg(msg);
4404 
4405  return targetDir + name;
4406 }
4407 
4408 void MainWindow::beep()
4409 {
4410  QApplication::beep();
4411 }
4412 
4413 void MainWindow::cancelProgress()
4414 {
4415  _progressCanceled = true;
4416  _progressDialog->appendText(tr("Canceled!"));
4417 }
4418 
4419 void MainWindow::configGUIModified()
4420 {
4421  this->setWindowModified(true);
4422 }
4423 
4424 void MainWindow::updateParameters(const ParametersMap & parameters)
4425 {
4426  if(parameters.size())
4427  {
4428  for(ParametersMap::const_iterator iter= parameters.begin(); iter!=parameters.end(); ++iter)
4429  {
4430  QString msg = tr("Parameter update \"%1\"=\"%2\"")
4431  .arg(iter->first.c_str())
4432  .arg(iter->second.c_str());
4433  _ui->widget_console->appendMsg(msg);
4434  UWARN(msg.toStdString().c_str());
4435  }
4436  QMessageBox::StandardButton button = QMessageBox::question(this,
4437  tr("Parameters"),
4438  tr("Some parameters have been set on command line, do you "
4439  "want to set all other RTAB-Map's parameters to default?"),
4440  QMessageBox::Yes | QMessageBox::No,
4441  QMessageBox::No);
4442  _preferencesDialog->updateParameters(parameters, button==QMessageBox::Yes);
4443  }
4444 }
4445 
4446 //ACTIONS
4447 void MainWindow::saveConfigGUI()
4448 {
4449  _savedMaximized = this->isMaximized();
4450  _preferencesDialog->saveMainWindowState(this);
4451  _preferencesDialog->saveWindowGeometry(_preferencesDialog);
4452  _preferencesDialog->saveWindowGeometry(_aboutDialog);
4453  _preferencesDialog->saveWidgetState(_cloudViewer);
4454  _preferencesDialog->saveWidgetState(_ui->imageView_source);
4455  _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
4456  _preferencesDialog->saveWidgetState(_ui->imageView_odometry);
4457  _preferencesDialog->saveWidgetState(_exportCloudsDialog);
4458  _preferencesDialog->saveWidgetState(_exportBundlerDialog);
4459  _preferencesDialog->saveWidgetState(_postProcessingDialog);
4460  _preferencesDialog->saveWidgetState(_depthCalibrationDialog);
4461  _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
4462  _preferencesDialog->saveSettings();
4463  this->saveFigures();
4464  this->setWindowModified(false);
4465 }
4466 
4467 void MainWindow::newDatabase()
4468 {
4469  if(_state != MainWindow::kIdle)
4470  {
4471  UERROR("This method can be called only in IDLE state.");
4472  return;
4473  }
4474  _openedDatabasePath.clear();
4475  _newDatabasePath.clear();
4476  _newDatabasePathOutput.clear();
4477  _databaseUpdated = false;
4478  ULOGGER_DEBUG("");
4479  this->clearTheCache();
4480  std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
4481  if(QFile::exists(databasePath.c_str()))
4482  {
4483  int r = QMessageBox::question(this,
4484  tr("Creating temporary database"),
4485  tr("Cannot create a new database because the temporary database \"%1\" already exists. "
4486  "There may be another instance of RTAB-Map running with the same Working Directory or "
4487  "the last time RTAB-Map was not closed correctly. "
4488  "Do you want to recover the database (click Ignore to delete it and create a new one)?").arg(databasePath.c_str()),
4489  QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::No);
4490 
4491  if(r == QMessageBox::Ignore)
4492  {
4493  if(QFile::remove(databasePath.c_str()))
4494  {
4495  UINFO("Deleted temporary database \"%s\".", databasePath.c_str());
4496  }
4497  else
4498  {
4499  UERROR("Temporary database \"%s\" could not be deleted!", databasePath.c_str());
4500  return;
4501  }
4502  }
4503  else if(r == QMessageBox::Yes)
4504  {
4505  std::string errorMsg;
4506  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4507  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4508  progressDialog->setMaximumSteps(100);
4509  progressDialog->show();
4510  progressDialog->setCancelButtonVisible(true);
4511  RecoveryState state(progressDialog);
4512  _recovering = true;
4513  if(databaseRecovery(databasePath, false, &errorMsg, &state))
4514  {
4515  _recovering = false;
4516  progressDialog->setValue(progressDialog->maximumSteps());
4517  QString newPath = QFileDialog::getSaveFileName(this, tr("Save recovered database"), _preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("recovered.db"), tr("RTAB-Map database files (*.db)"));
4518  if(newPath.isEmpty())
4519  {
4520  return;
4521  }
4522  if(QFileInfo(newPath).suffix() == "")
4523  {
4524  newPath += ".db";
4525  }
4526  if(QFile::exists(newPath))
4527  {
4528  QFile::remove(newPath);
4529  }
4530  QFile::rename(databasePath.c_str(), newPath);
4531  return;
4532  }
4533  else
4534  {
4535  _recovering = false;
4536  progressDialog->setValue(progressDialog->maximumSteps());
4537  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
4538  return;
4539  }
4540  }
4541  else
4542  {
4543  return;
4544  }
4545  }
4546  _newDatabasePath = databasePath.c_str();
4547  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
4548  applyPrefSettings(_preferencesDialog->getAllParameters(), false);
4549 }
4550 
4551 void MainWindow::openDatabase()
4552 {
4553  QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
4554  if(!path.isEmpty())
4555  {
4556  this->openDatabase(path);
4557  }
4558 }
4559 
4560 void MainWindow::openDatabase(const QString & path, const ParametersMap & overridedParameters)
4561 {
4562  if(_state != MainWindow::kIdle)
4563  {
4564  UERROR("Database can only be opened in IDLE state.");
4565  return;
4566  }
4567 
4568  std::string value = path.toStdString();
4569  if(UFile::exists(value) &&
4570  UFile::getExtension(value).compare("db") == 0)
4571  {
4572  _openedDatabasePath.clear();
4573  _newDatabasePath.clear();
4574  _newDatabasePathOutput.clear();
4575  _databaseUpdated = false;
4576 
4577  this->clearTheCache();
4578  _openedDatabasePath = path;
4579 
4580  // look if there are saved parameters
4581  DBDriver * driver = DBDriver::create();
4582  if(driver->openConnection(value, false))
4583  {
4584  ParametersMap parameters = driver->getLastParameters();
4585  driver->closeConnection(false);
4586  delete driver;
4587 
4588  if(parameters.size())
4589  {
4590  //backward compatibility with databases not saving all parameters, use default for not saved ones
4591  for(ParametersMap::const_iterator iter=Parameters::getDefaultParameters().begin(); iter!=Parameters::getDefaultParameters().end(); ++iter)
4592  {
4593  parameters.insert(*iter);
4594  }
4595 
4596  uInsert(parameters, overridedParameters);
4597 
4598  ParametersMap currentParameters = _preferencesDialog->getAllParameters();
4599  ParametersMap differentParameters;
4600  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
4601  {
4602  ParametersMap::iterator jter = currentParameters.find(iter->first);
4603  if(jter!=currentParameters.end() &&
4604  iter->second.compare(jter->second) != 0 &&
4605  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
4606  {
4607  bool different = true;
4608  if(Parameters::getType(iter->first).compare("double") ==0 ||
4609  Parameters::getType(iter->first).compare("float") == 0)
4610  {
4611  if(uStr2Double(iter->second) == uStr2Double(jter->second))
4612  {
4613  different = false;
4614  }
4615  }
4616  if(different)
4617  {
4618  differentParameters.insert(*iter);
4619  QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
4620  .arg(iter->first.c_str())
4621  .arg(iter->second.c_str())
4622  .arg(jter->second.c_str());
4623  _ui->widget_console->appendMsg(msg);
4624  UWARN(msg.toStdString().c_str());
4625  }
4626  }
4627  }
4628 
4629  if(differentParameters.size())
4630  {
4631  int r = QMessageBox::question(this,
4632  tr("Update parameters..."),
4633  tr("The database is using %1 different parameter(s) than "
4634  "those currently set in Preferences. Do you want "
4635  "to use database's parameters?").arg(differentParameters.size()),
4636  QMessageBox::Yes | QMessageBox::No,
4637  QMessageBox::Yes);
4638  if(r == QMessageBox::Yes)
4639  {
4640  _preferencesDialog->updateParameters(differentParameters);
4641  }
4642  }
4643  }
4644  }
4645 
4646  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, value, 0, _preferencesDialog->getAllParameters()));
4647  applyPrefSettings(_preferencesDialog->getAllParameters(), false);
4648  }
4649  else
4650  {
4651  UERROR("File \"%s\" not valid.", value.c_str());
4652  }
4653 }
4654 
4655 bool MainWindow::closeDatabase()
4656 {
4657  if(_state != MainWindow::kInitialized)
4658  {
4659  UERROR("This method can be called only in INITIALIZED state.");
4660  return false;
4661  }
4662 
4663  _newDatabasePathOutput.clear();
4664  if(!_newDatabasePath.isEmpty() && _databaseUpdated)
4665  {
4666  QMessageBox::Button b = QMessageBox::question(this,
4667  tr("Save database"),
4668  tr("Save the new database?"),
4669  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard,
4670  QMessageBox::Save);
4671 
4672  if(b == QMessageBox::Save)
4673  {
4674  // Temp database used, automatically backup with unique name (timestamp)
4675  QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
4676  QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
4677 
4678  newPath = QFileDialog::getSaveFileName(this, tr("Save database"), newPath, tr("RTAB-Map database files (*.db)"));
4679  if(newPath.isEmpty())
4680  {
4681  return false;
4682  }
4683 
4684  if(QFileInfo(newPath).suffix() == "")
4685  {
4686  newPath += ".db";
4687  }
4688 
4689  _newDatabasePathOutput = newPath;
4690  }
4691  else if(b != QMessageBox::Discard)
4692  {
4693  return false;
4694  }
4695  }
4696 
4697  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_openedDatabasePath.isEmpty() || !_newDatabasePathOutput.isEmpty()));
4698  return true;
4699 }
4700 
4701 void MainWindow::editDatabase()
4702 {
4703  if(_state != MainWindow::kIdle)
4704  {
4705  UERROR("This method can be called only in IDLE state.");
4706  return;
4707  }
4708  QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
4709  if(!path.isEmpty())
4710  {
4711  {
4712  // copy database settings to tmp ini file
4713  QSettings dbSettingsIn(_preferencesDialog->getIniFilePath(), QSettings::IniFormat);
4714  QSettings dbSettingsOut(_preferencesDialog->getTmpIniFilePath(), QSettings::IniFormat);
4715  dbSettingsIn.beginGroup("DatabaseViewer");
4716  dbSettingsOut.beginGroup("DatabaseViewer");
4717  QStringList keys = dbSettingsIn.childKeys();
4718  for(QStringList::iterator iter = keys.begin(); iter!=keys.end(); ++iter)
4719  {
4720  dbSettingsOut.setValue(*iter, dbSettingsIn.value(*iter));
4721  }
4722  dbSettingsIn.endGroup();
4723  dbSettingsOut.endGroup();
4724  }
4725 
4726  DatabaseViewer * viewer = new DatabaseViewer(_preferencesDialog->getTmpIniFilePath(), this);
4727  viewer->setWindowModality(Qt::WindowModal);
4728  viewer->setAttribute(Qt::WA_DeleteOnClose, true);
4729  viewer->showCloseButton();
4730 
4731  if(viewer->isSavedMaximized())
4732  {
4733  viewer->showMaximized();
4734  }
4735  else
4736  {
4737  viewer->show();
4738  }
4739 
4740  viewer->openDatabase(path);
4741  }
4742 }
4743 
4744 void MainWindow::startDetection()
4745 {
4746  UDEBUG("");
4747  ParametersMap parameters = _preferencesDialog->getAllParameters();
4748  // verify source with input rates
4749  if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
4750  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
4751  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
4752  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
4753  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
4754  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
4755  {
4756  float inputRate = _preferencesDialog->getGeneralInputRate();
4757  float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
4758  int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
4759  if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) &&
4760  (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed()))
4761  {
4762  int button = QMessageBox::question(this,
4763  tr("Incompatible frame rates!"),
4764  tr("\"Source/Input rate\" (%1 Hz) is equal to/higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the "
4765  "source input is a directory of images/video/database, some images may be "
4766  "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over "
4767  "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to "
4768  "start the detection anyway?").arg(inputRate).arg(detectionRate),
4769  QMessageBox::Yes | QMessageBox::No);
4770  if(button == QMessageBox::No)
4771  {
4772  return;
4773  }
4774  }
4775  if(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed())
4776  {
4777  if(bufferingSize != 0)
4778  {
4779  int button = QMessageBox::question(this,
4780  tr("Some images may be skipped!"),
4781  tr("\"RTAB-Map/Images buffer size\" is not infinite (size=%1). As the "
4782  "source input is a directory of images/video/database, some images may be "
4783  "skipped by the detector if the \"Source/Input rate\" (which is %2 Hz) is higher than the "
4784  "rate at which RTAB-Map can process the images. You may want to set the "
4785  "\"RTAB-Map/Images buffer size\" to 0 (infinite) to guaranty that all "
4786  "images are processed. Would you want to start the detection "
4787  "anyway?").arg(bufferingSize).arg(inputRate),
4788  QMessageBox::Yes | QMessageBox::No);
4789  if(button == QMessageBox::No)
4790  {
4791  return;
4792  }
4793  }
4794  else if(inputRate == 0)
4795  {
4796  int button = QMessageBox::question(this,
4797  tr("Large number of images may be buffered!"),
4798  tr("\"RTAB-Map/Images buffer size\" is infinite. As the "
4799  "source input is a directory of images/video/database and "
4800  "that \"Source/Input rate\" is infinite too, a lot of images "
4801  "could be buffered at the same time (e.g., reading all images "
4802  "of a directory at once). This could make the GUI not responsive. "
4803  "You may want to set \"Source/Input rate\" at the rate at "
4804  "which the images have been recorded. "
4805  "Would you want to start the detection "
4806  "anyway?").arg(bufferingSize).arg(inputRate),
4807  QMessageBox::Yes | QMessageBox::No);
4808  if(button == QMessageBox::No)
4809  {
4810  return;
4811  }
4812  }
4813  }
4814  }
4815 
4816  UDEBUG("");
4817  Q_EMIT stateChanged(kStartingDetection);
4818 
4819  if(_camera != 0)
4820  {
4821  QMessageBox::warning(this,
4822  tr("RTAB-Map"),
4823  tr("A camera is running, stop it first."));
4824  UWARN("_camera is not null... it must be stopped first");
4825  Q_EMIT stateChanged(kInitialized);
4826  return;
4827  }
4828 
4829  // Adjust pre-requirements
4830  if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
4831  {
4832  QMessageBox::warning(this,
4833  tr("RTAB-Map"),
4834  tr("No sources are selected. See Preferences->Source panel."));
4835  UWARN("No sources are selected. See Preferences->Source panel.");
4836  Q_EMIT stateChanged(kInitialized);
4837  return;
4838  }
4839 
4840 
4841  Camera * camera = _preferencesDialog->createCamera();
4842  if(!camera)
4843  {
4844  Q_EMIT stateChanged(kInitialized);
4845  return;
4846  }
4847 
4848  _camera = new CameraThread(camera, parameters);
4849  _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
4850  _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
4851  _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
4852  _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
4853  _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation());
4854  _camera->setScanFromDepth(
4855  _preferencesDialog->isSourceScanFromDepth(),
4856  _preferencesDialog->getSourceScanFromDepthDecimation(),
4857  _preferencesDialog->getSourceScanFromDepthMaxDepth(),
4858  _preferencesDialog->getSourceScanVoxelSize(),
4859  _preferencesDialog->getSourceScanNormalsK(),
4860  _preferencesDialog->getSourceScanNormalsRadius());
4861  if(_preferencesDialog->isDepthFilteringAvailable())
4862  {
4863  if(_preferencesDialog->isBilateralFiltering())
4864  {
4865  _camera->enableBilateralFiltering(
4866  _preferencesDialog->getBilateralSigmaS(),
4867  _preferencesDialog->getBilateralSigmaR());
4868  }
4869  _camera->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString());
4870  }
4871 
4872  //Create odometry thread if rgbd slam
4873  if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
4874  {
4875  // Require calibrated camera
4876  if(!camera->isCalibrated())
4877  {
4878  UWARN("Camera is not calibrated!");
4879  Q_EMIT stateChanged(kInitialized);
4880  delete _camera;
4881  _camera = 0;
4882 
4883  int button = QMessageBox::question(this,
4884  tr("Camera is not calibrated!"),
4885  tr("RTAB-Map in metric SLAM mode cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
4886  QMessageBox::Yes | QMessageBox::No);
4887  if(button == QMessageBox::Yes)
4888  {
4889  QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
4890  }
4891  return;
4892  }
4893  else
4894  {
4895  if(_odomThread)
4896  {
4897  UERROR("OdomThread must be already deleted here?!");
4898  delete _odomThread;
4899  _odomThread = 0;
4900  }
4901 
4902  if(_imuThread)
4903  {
4904  UERROR("ImuThread must be already deleted here?!");
4905  delete _imuThread;
4906  _imuThread = 0;
4907  }
4908 
4909  if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
4910  {
4911  ParametersMap odomParameters = parameters;
4912  if(_preferencesDialog->getOdomRegistrationApproach() < 3)
4913  {
4914  uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
4915  }
4916  odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
4917  int odomStrategy = Parameters::defaultOdomStrategy();
4918  Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
4919  if(odomStrategy == 1)
4920  {
4921  // Only Frame To Frame supports all VisCorType
4922  odomParameters.insert(ParametersPair(Parameters::kVisCorType(), _preferencesDialog->getParameter(Parameters::kVisCorType())));
4923  }
4924  _imuThread = 0;
4925  if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
4926  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
4927  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
4928  !_preferencesDialog->getIMUPath().isEmpty())
4929  {
4930  if(odomStrategy != Odometry::kTypeOkvis && odomStrategy != Odometry::kTypeMSCKF)
4931  {
4932  QMessageBox::warning(this, tr("Source IMU Path"),
4933  tr("IMU path is set but odometry chosen doesn't support IMU, ignoring IMU..."), QMessageBox::Ok);
4934  }
4935  else
4936  {
4937  _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
4938  if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
4939  {
4940  QMessageBox::warning(this, tr("Source IMU Path"),
4941  tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
4942  delete _camera;
4943  _camera = 0;
4944  delete _imuThread;
4945  _imuThread = 0;
4946  return;
4947  }
4948  }
4949  }
4950  Odometry * odom = Odometry::create(odomParameters);
4951  _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
4952 
4953  UEventsManager::addHandler(_odomThread);
4954  UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
4955  UEventsManager::createPipe(_camera, this, "CameraEvent");
4956  if(_imuThread)
4957  {
4958  UEventsManager::createPipe(_imuThread, _odomThread, "IMUEvent");
4959  }
4960  _odomThread->start();
4961  }
4962  }
4963  }
4964 
4965  if(_dataRecorder && _camera && _odomThread)
4966  {
4967  UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
4968  }
4969 
4970  _lastOdomPose.setNull();
4971  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer)); // clean sensors buffer
4972  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap)); // Trigger a new map
4973 
4974  if(_odomThread)
4975  {
4976  _ui->actionReset_Odometry->setEnabled(true);
4977  }
4978 
4979  if(!_preferencesDialog->isStatisticsPublished())
4980  {
4981  QMessageBox::information(this,
4982  tr("Information"),
4983  tr("Note that publishing statistics is disabled, "
4984  "progress will not be shown in the GUI."));
4985  }
4986 
4987  _occupancyGrid->clear();
4988  _occupancyGrid->parseParameters(parameters);
4989 
4990 #ifdef RTABMAP_OCTOMAP
4991  UASSERT(_octomap != 0);
4992  delete _octomap;
4993  _octomap = new OctoMap(parameters);
4994 #endif
4995 
4996  // clear odometry visual stuff
4997  _cloudViewer->removeCloud("cloudOdom");
4998  _cloudViewer->removeCloud("scanOdom");
4999  _cloudViewer->removeCloud("scanMapOdom");
5000  _cloudViewer->removeCloud("featuresOdom");
5001  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
5002 
5003  Q_EMIT stateChanged(kDetecting);
5004 }
5005 
5006 // Could not be in the main thread here! (see handleEvents())
5007 void MainWindow::pauseDetection()
5008 {
5009  if(_camera)
5010  {
5011  if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
5012  {
5013  // On Ctrl-click, start the camera and pause it automatically
5014  Q_EMIT stateChanged(kPaused);
5015  if(_preferencesDialog->getGeneralInputRate())
5016  {
5017  QTimer::singleShot(500.0/_preferencesDialog->getGeneralInputRate(), this, SLOT(pauseDetection()));
5018  }
5019  else
5020  {
5021  Q_EMIT stateChanged(kPaused);
5022  }
5023  }
5024  else
5025  {
5026  Q_EMIT stateChanged(kPaused);
5027  }
5028  }
5029  else if(_state == kMonitoring)
5030  {
5031  UINFO("Sending pause event!");
5032  Q_EMIT stateChanged(kMonitoringPaused);
5033  }
5034  else if(_state == kMonitoringPaused)
5035  {
5036  UINFO("Sending unpause event!");
5037  Q_EMIT stateChanged(kMonitoring);
5038  }
5039 }
5040 
5041 void MainWindow::stopDetection()
5042 {
5043  if(!_camera && !_odomThread)
5044  {
5045  return;
5046  }
5047 
5048  if(_state == kDetecting &&
5049  (_camera && _camera->isRunning()) )
5050  {
5051  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);
5052 
5053  if(button != QMessageBox::Yes)
5054  {
5055  return;
5056  }
5057  }
5058 
5059  ULOGGER_DEBUG("");
5060  // kill the processes
5061  if(_imuThread)
5062  {
5063  _imuThread->join(true);
5064  }
5065 
5066  if(_camera)
5067  {
5068  _camera->join(true);
5069  }
5070 
5071  if(_odomThread)
5072  {
5073  _ui->actionReset_Odometry->setEnabled(false);
5074  _odomThread->kill();
5075  }
5076 
5077  // delete the processes
5078  if(_imuThread)
5079  {
5080  delete _imuThread;
5081  _imuThread = 0;
5082  }
5083  if(_camera)
5084  {
5085  delete _camera;
5086  _camera = 0;
5087  }
5088  if(_odomThread)
5089  {
5090  delete _odomThread;
5091  _odomThread = 0;
5092  }
5093 
5094  if(_dataRecorder)
5095  {
5096  delete _dataRecorder;
5097  _dataRecorder = 0;
5098  }
5099 
5100  Q_EMIT stateChanged(kInitialized);
5101 }
5102 
5103 void MainWindow::notifyNoMoreImages()
5104 {
5105  QMessageBox::information(this,
5106  tr("No more images..."),
5107  tr("The camera has reached the end of the stream."));
5108 }
5109 
5110 void MainWindow::printLoopClosureIds()
5111 {
5112  _ui->dockWidget_console->show();
5113  QString msgRef;
5114  QString msgLoop;
5115  for(int i = 0; i<_refIds.size(); ++i)
5116  {
5117  msgRef.append(QString::number(_refIds[i]));
5118  msgLoop.append(QString::number(_loopClosureIds[i]));
5119  if(i < _refIds.size() - 1)
5120  {
5121  msgRef.append(" ");
5122  msgLoop.append(" ");
5123  }
5124  }
5125  _ui->widget_console->appendMsg(QString("IDs = [%1];").arg(msgRef));
5126  _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
5127 }
5128 
5129 void MainWindow::generateGraphDOT()
5130 {
5131  if(_graphSavingFileName.isEmpty())
5132  {
5133  _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
5134  }
5135 
5136  bool ok;
5137  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
5138  if(ok)
5139  {
5140  int margin = 0;
5141  if(id > 0)
5142  {
5143  margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
5144  }
5145 
5146  if(ok)
5147  {
5148  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
5149  if(!path.isEmpty())
5150  {
5151  _graphSavingFileName = path;
5152  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
5153 
5154  _ui->dockWidget_console->show();
5155  _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName));
5156  }
5157  }
5158  }
5159 }
5160 
5161 void MainWindow::exportPosesRaw()
5162 {
5163  exportPoses(0);
5164 }
5165 void MainWindow::exportPosesRGBDSLAM()
5166 {
5167  exportPoses(1);
5168 }
5169 void MainWindow::exportPosesKITTI()
5170 {
5171  exportPoses(2);
5172 }
5173 void MainWindow::exportPosesTORO()
5174 {
5175  exportPoses(3);
5176 }
5177 void MainWindow::exportPosesG2O()
5178 {
5179  exportPoses(4);
5180 }
5181 
5182 void MainWindow::exportPoses(int format)
5183 {
5184  if(_currentPosesMap.size())
5185  {
5186  std::map<int, Transform> localTransforms;
5187  QStringList items;
5188  items.push_back("Robot");
5189  items.push_back("Camera");
5190  items.push_back("Scan");
5191  bool ok;
5192  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false, &ok);
5193  if(!ok || item.isEmpty())
5194  {
5195  return;
5196  }
5197  if(item.compare("Robot") != 0)
5198  {
5199  bool cameraFrame = item.compare("Camera") == 0;
5200  _exportPosesFrame = cameraFrame?1:2;
5201  for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
5202  {
5203  if(_cachedSignatures.contains(iter->first))
5204  {
5205  Transform localTransform;
5206  if(cameraFrame)
5207  {
5208  if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
5209  !_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull()))
5210  {
5211  localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
5212  }
5213  else if(!_cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform().isNull())
5214  {
5215  localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform();
5216  }
5217  else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1)
5218  {
5219  UWARN("Multi-camera is not supported (node %d)", iter->first);
5220  }
5221  else
5222  {
5223  UWARN("Missing calibration for node %d", iter->first);
5224  }
5225  }
5226  else
5227  {
5228  if(!_cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform().isNull())
5229  {
5230  localTransform = _cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform();
5231  }
5232  else if(!_cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform().isNull())
5233  {
5234  localTransform = _cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform();
5235  }
5236  else
5237  {
5238  UWARN("Missing scan info for node %d", iter->first);
5239  }
5240  }
5241  if(!localTransform.isNull())
5242  {
5243  localTransforms.insert(std::make_pair(iter->first, localTransform));
5244  }
5245  }
5246  else
5247  {
5248  UWARN("Did not find node %d in cache", iter->first);
5249  }
5250  }
5251  if(localTransforms.empty())
5252  {
5253  QMessageBox::warning(this,
5254  tr("Export Poses"),
5255  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
5256  }
5257  }
5258  else
5259  {
5260  _exportPosesFrame = 0;
5261  }
5262 
5263  std::map<int, Transform> poses;
5264  std::multimap<int, Link> links;
5265  if(localTransforms.empty())
5266  {
5267  poses = _currentPosesMap;
5268  links = _currentLinksMap;
5269  }
5270  else
5271  {
5272  //adjust poses and links
5273  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
5274  {
5275  poses.insert(std::make_pair(iter->first, _currentPosesMap.at(iter->first) * iter->second));
5276  }
5277  for(std::multimap<int, Link>::iterator iter=_currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter)
5278  {
5279  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
5280  {
5281  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
5282  int from = iter->second.from();
5283  int to = iter->second.to();
5284  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
5285  }
5286  }
5287  }
5288 
5289  std::map<int, double> stamps;
5290  if(format == 1)
5291  {
5292  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
5293  {
5294  if(_cachedSignatures.contains(iter->first))
5295  {
5296  stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
5297  }
5298  }
5299  if(stamps.size()!=poses.size())
5300  {
5301  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.")
5302  .arg(poses.size()).arg(stamps.size()));
5303  return;
5304  }
5305  }
5306 
5307  if(_exportPosesFileName[format].isEmpty())
5308  {
5309  _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
5310  }
5311 
5312  QString path = QFileDialog::getSaveFileName(
5313  this,
5314  tr("Save File"),
5315  _exportPosesFileName[format],
5316  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
5317 
5318  if(!path.isEmpty())
5319  {
5320  if(QFileInfo(path).suffix() == "")
5321  {
5322  if(format == 3)
5323  {
5324  path += ".graph";
5325  }
5326  else if(format==4)
5327  {
5328  path += ".g2o";
5329  }
5330  else
5331  {
5332  path += ".txt";
5333  }
5334  }
5335 
5336  _exportPosesFileName[format] = path;
5337  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
5338 
5339  if(saved)
5340  {
5341  QMessageBox::information(this,
5342  tr("Export poses..."),
5343  tr("%1 saved to \"%2\".")
5344  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
5345  .arg(_exportPosesFileName[format]));
5346  }
5347  else
5348  {
5349  QMessageBox::information(this,
5350  tr("Export poses..."),
5351  tr("Failed to save %1 to \"%2\"!")
5352  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
5353  .arg(_exportPosesFileName[format]));
5354  }
5355  }
5356  }
5357 }
5358 
5359 void MainWindow::postProcessing()
5360 {
5361  if(_cachedSignatures.size() == 0)
5362  {
5363  QMessageBox::warning(this,
5364  tr("Post-processing..."),
5365  tr("Signatures must be cached in the GUI for post-processing. "
5366  "Check the option in Preferences->General Settings (GUI), then "
5367  "refresh the cache."));
5368  return;
5369  }
5370  if(_postProcessingDialog->exec() != QDialog::Accepted)
5371  {
5372  return;
5373  }
5374 
5375  bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
5376  bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
5377  bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
5378  double clusterRadius = _postProcessingDialog->clusterRadius();
5379  double clusterAngle = _postProcessingDialog->clusterAngle();
5380  int detectLoopClosureIterations = _postProcessingDialog->iterations();
5381  bool sba = _postProcessingDialog->isSBA();
5382  int sbaIterations = _postProcessingDialog->sbaIterations();
5383  double sbaVariance = _postProcessingDialog->sbaVariance();
5384  Optimizer::Type sbaType = _postProcessingDialog->sbaType();
5385 
5386  if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
5387  {
5388  UWARN("No post-processing selection...");
5389  return;
5390  }
5391 
5392  // First, verify that we have all data required in the GUI
5393  bool allDataAvailable = true;
5394  std::map<int, Transform> odomPoses;
5395  for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
5396  iter!=_currentPosesMap.end() && allDataAvailable;
5397  ++iter)
5398  {
5399  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
5400  if(jter != _cachedSignatures.end())
5401  {
5402  if(jter->getPose().isNull())
5403  {
5404  UWARN("Odometry pose of %d is null.", iter->first);
5405  allDataAvailable = false;
5406  }
5407  else
5408  {
5409  odomPoses.insert(*iter); // fill raw poses
5410  }
5411  }
5412  else
5413  {
5414  UWARN("Node %d missing.", iter->first);
5415  allDataAvailable = false;
5416  }
5417  }
5418 
5419  if(!allDataAvailable)
5420  {
5421  QMessageBox::warning(this, tr("Not all data available in the GUI..."),
5422  tr("Some data missing in the cache to respect the constraints chosen. "
5423  "Try \"Edit->Download all clouds\" to update the cache and try again."));
5424  return;
5425  }
5426 
5427  _progressDialog->resetProgress();
5428  _progressDialog->clear();
5429  _progressDialog->show();
5430  _progressDialog->appendText("Post-processing beginning!");
5431  _progressDialog->setCancelButtonVisible(true);
5432  _progressCanceled = false;
5433 
5434  int totalSteps = 0;
5435  if(refineNeighborLinks)
5436  {
5437  totalSteps+=(int)odomPoses.size();
5438  }
5439  if(refineLoopClosureLinks)
5440  {
5441  totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
5442  }
5443  if(sba)
5444  {
5445  totalSteps+=1;
5446  }
5447  _progressDialog->setMaximumSteps(totalSteps);
5448  _progressDialog->show();
5449 
5450  ParametersMap parameters = _preferencesDialog->getAllParameters();
5451  Optimizer * optimizer = Optimizer::create(parameters);
5452  bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
5453  float optimizeMaxError = Parameters::defaultRGBDOptimizeMaxError();
5454  int optimizeIterations = Parameters::defaultOptimizerIterations();
5455  bool reextractFeatures = Parameters::defaultRGBDLoopClosureReextractFeatures();
5456  Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
5457  Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), optimizeMaxError);
5458  Parameters::parse(parameters, Parameters::kOptimizerIterations(), optimizeIterations);
5459  Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), reextractFeatures);
5460 
5461  bool warn = false;
5462  int loopClosuresAdded = 0;
5463  std::multimap<int, int> checkedLoopClosures;
5464  if(detectMoreLoopClosures)
5465  {
5466  UDEBUG("");
5467 
5468  UASSERT(detectLoopClosureIterations>0);
5469  for(int n=0; n<detectLoopClosureIterations && !_progressCanceled; ++n)
5470  {
5471  _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
5472  .arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
5473 
5474  std::multimap<int, int> clusters = graph::radiusPosesClustering(
5475  _currentPosesMap,
5476  clusterRadius,
5477  clusterAngle*CV_PI/180.0);
5478 
5479  _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+(int)clusters.size());
5480  _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
5481  QApplication::processEvents();
5482 
5483  int i=0;
5484  std::set<int> addedLinks;
5485  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)
5486  {
5487  int from = iter->first;
5488  int to = iter->second;
5489  if(iter->first < iter->second)
5490  {
5491  from = iter->second;
5492  to = iter->first;
5493  }
5494 
5495  bool alreadyChecked = false;
5496  for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
5497  !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
5498  ++jter)
5499  {
5500  if(to == jter->second)
5501  {
5502  alreadyChecked = true;
5503  }
5504  }
5505 
5506  if(!alreadyChecked)
5507  {
5508  // only add new links and one per cluster per iteration
5509  if(addedLinks.find(from) == addedLinks.end() &&
5510  rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
5511  {
5512  checkedLoopClosures.insert(std::make_pair(from, to));
5513 
5514  if(!_cachedSignatures.contains(from))
5515  {
5516  UERROR("Didn't find signature %d", from);
5517  }
5518  else if(!_cachedSignatures.contains(to))
5519  {
5520  UERROR("Didn't find signature %d", to);
5521  }
5522  else
5523  {
5524  Signature signatureFrom = _cachedSignatures[from];
5525  Signature signatureTo = _cachedSignatures[to];
5526 
5527  if(signatureFrom.getWeight() >= 0 &&
5528  signatureTo.getWeight() >= 0) // ignore intermediate nodes
5529  {
5530  Transform transform;
5531  RegistrationInfo info;
5532  if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
5533  parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
5534  {
5535  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
5536  }
5537  Registration * registration = Registration::create(parameters);
5538 
5539  if(reextractFeatures)
5540  {
5541  signatureFrom.sensorData().uncompressData();
5542  signatureTo.sensorData().uncompressData();
5543 
5544  if(signatureFrom.sensorData().imageRaw().empty() &&
5545  signatureTo.sensorData().imageRaw().empty())
5546  {
5547  UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
5548  "images. Update the cache.",
5549  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
5550  }
5551  else
5552  {
5553  signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
5554  signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
5555  signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
5556  signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
5557  signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
5558  signatureTo.setWords3(std::multimap<int, cv::Point3f>());
5559  signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
5560  signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
5561  }
5562  }
5563  else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
5564  {
5565  UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
5566  "registration will not be possible. Set \"%s\" to true.",
5567  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
5568  signatureFrom.id(),
5569  signatureTo.id(),
5570  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
5571  }
5572  transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
5573  delete registration;
5574  if(!transform.isNull())
5575  {
5576  if(!transform.isIdentity())
5577  {
5578  // normalize variance
5579  info.covariance *= transform.getNorm();
5580  if(info.covariance.at<double>(0,0)<=0.0)
5581  {
5582  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
5583  }
5584  }
5585 
5586  //optimize the graph to see if the new constraint is globally valid
5587  bool updateConstraint = true;
5588  if(optimizeMaxError > 0.0f && optimizeIterations > 0)
5589  {
5590  int fromId = from;
5591  int mapId = _currentMapIds.at(from);
5592  // use first node of the map containing from
5593  for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
5594  {
5595  if(iter->second == mapId && odomPoses.find(iter->first)!=odomPoses.end())
5596  {
5597  fromId = iter->first;
5598  break;
5599  }
5600  }
5601  std::multimap<int, Link> linksIn = _currentLinksMap;
5602  linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
5603  const Link * maxLinearLink = 0;
5604  const Link * maxAngularLink = 0;
5605  float maxLinearError = 0.0f;
5606  float maxAngularError = 0.0f;
5607  std::map<int, Transform> poses;
5608  std::multimap<int, Link> links;
5609  UASSERT(odomPoses.find(fromId) != odomPoses.end());
5610  UASSERT_MSG(odomPoses.find(from) != odomPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
5611  UASSERT_MSG(odomPoses.find(to) != odomPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
5612  optimizer->getConnectedGraph(fromId, odomPoses, linksIn, poses, links);
5613  UASSERT(poses.find(fromId) != poses.end());
5614  UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
5615  UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
5616  UASSERT(graph::findLink(links, from, to) != links.end());
5617  poses = optimizer->optimize(fromId, poses, links);
5618  std::string msg;
5619  if(poses.size())
5620  {
5621  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5622  {
5623  // ignore links with high variance
5624  if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
5625  {
5626  UASSERT(poses.find(iter->second.from())!=poses.end());
5627  UASSERT(poses.find(iter->second.to())!=poses.end());
5628  Transform t1 = poses.at(iter->second.from());
5629  Transform t2 = poses.at(iter->second.to());
5630  UASSERT(!t1.isNull() && !t2.isNull());
5631  Transform t = t1.inverse()*t2;
5632  float linearError = uMax3(
5633  fabs(iter->second.transform().x() - t.x()),
5634  fabs(iter->second.transform().y() - t.y()),
5635  fabs(iter->second.transform().z() - t.z()));
5636  Eigen::Vector3f vA = t1.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
5637  Eigen::Vector3f vB = t2.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
5638  float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
5639  if(linearError > maxLinearError)
5640  {
5641  maxLinearError = linearError;
5642  maxLinearLink = &iter->second;
5643  }
5644  if(angularError > maxAngularError)
5645  {
5646  maxAngularError = angularError;
5647  maxAngularLink = &iter->second;
5648  }
5649  }
5650  }
5651  if(maxLinearLink)
5652  {
5653  UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
5654  }
5655  if(maxAngularLink)
5656  {
5657  UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
5658  }
5659 
5660  if(maxLinearError > optimizeMaxError)
5661  {
5662  msg = uFormat("Rejecting edge %d->%d because "
5663  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
5664  "\"%s\" is %f m.",
5665  from,
5666  to,
5667  maxLinearError,
5668  maxLinearLink->from(),
5669  maxLinearLink->to(),
5670  maxAngularError*180.0f/M_PI,
5671  maxAngularLink?maxAngularLink->from():0,
5672  maxAngularLink?maxAngularLink->to():0,
5673  Parameters::kRGBDOptimizeMaxError().c_str(),
5674  optimizeMaxError);
5675  }
5676  }
5677  else
5678  {
5679  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
5680  from,
5681  to);
5682  }
5683  if(!msg.empty())
5684  {
5685  UWARN("%s", msg.c_str());
5686  _progressDialog->appendText(tr("%1").arg(msg.c_str()));
5687  QApplication::processEvents();
5688  updateConstraint = false;
5689  }
5690  }
5691 
5692  if(updateConstraint)
5693  {
5694  UINFO("Added new loop closure between %d and %d.", from, to);
5695  addedLinks.insert(from);
5696  addedLinks.insert(to);
5697 
5698  _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
5699  ++loopClosuresAdded;
5700  _progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
5701  }
5702  }
5703  }
5704  }
5705  }
5706  }
5707  QApplication::processEvents();
5708  _progressDialog->incrementStep();
5709  }
5710  _progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
5711  if(addedLinks.size() == 0)
5712  {
5713  break;
5714  }
5715 
5716  if(n+1 < detectLoopClosureIterations)
5717  {
5718  _progressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
5719  .arg(odomPoses.size()).arg(_currentLinksMap.size()));
5720  QApplication::processEvents();
5721 
5722  int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
5723  std::map<int, rtabmap::Transform> posesOut;
5724  std::multimap<int, rtabmap::Link> linksOut;
5725  std::map<int, rtabmap::Transform> optimizedPoses;
5726  optimizer->getConnectedGraph(
5727  fromId,
5728  odomPoses,
5729  _currentLinksMap,
5730  posesOut,
5731  linksOut);
5732  optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
5733  _currentPosesMap = optimizedPoses;
5734  _progressDialog->appendText(tr("Optimizing graph with new links... done!"));
5735  }
5736  }
5737  UINFO("Added %d loop closures.", loopClosuresAdded);
5738  _progressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
5739  }
5740 
5741  if(!_progressCanceled && (refineNeighborLinks || refineLoopClosureLinks))
5742  {
5743  UDEBUG("");
5744  if(refineLoopClosureLinks)
5745  {
5746  _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+loopClosuresAdded);
5747  }
5748  // TODO: support ICP from laser scans?
5749  _progressDialog->appendText(tr("Refining links..."));
5750  QApplication::processEvents();
5751 
5752  RegistrationIcp regIcp(parameters);
5753 
5754  int i=0;
5755  for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end() && !_progressCanceled; ++iter, ++i)
5756  {
5757  int type = iter->second.type();
5758 
5759  if((refineNeighborLinks && type==Link::kNeighbor) ||
5760  (refineLoopClosureLinks && type!=Link::kNeighbor))
5761  {
5762  int from = iter->second.from();
5763  int to = iter->second.to();
5764 
5765  _progressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
5766  _progressDialog->incrementStep();
5767  QApplication::processEvents();
5768 
5769  if(!_cachedSignatures.contains(from))
5770  {
5771  UERROR("Didn't find signature %d",from);
5772  }
5773  else if(!_cachedSignatures.contains(to))
5774  {
5775  UERROR("Didn't find signature %d", to);
5776  }
5777  else
5778  {
5779  Signature & signatureFrom = _cachedSignatures[from];
5780  Signature & signatureTo = _cachedSignatures[to];
5781 
5782  LaserScan tmp;
5783  signatureFrom.sensorData().uncompressData(0,0,&tmp);
5784  signatureTo.sensorData().uncompressData(0,0,&tmp);
5785 
5786  if(!signatureFrom.sensorData().laserScanRaw().isEmpty() &&
5787  !signatureTo.sensorData().laserScanRaw().isEmpty())
5788  {
5789  RegistrationInfo info;
5790  Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
5791 
5792  if(!transform.isNull())
5793  {
5794  Link newLink(from, to, iter->second.type(), transform, info.covariance.inv());
5795  iter->second = newLink;
5796  }
5797  else
5798  {
5799  QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg.c_str());
5800  _progressDialog->appendText(str, Qt::darkYellow);
5801  UWARN("%s", str.toStdString().c_str());
5802  warn = true;
5803  }
5804  }
5805  else
5806  {
5807  QString str;
5808  if(signatureFrom.getWeight() < 0 || signatureTo.getWeight() < 0)
5809  {
5810  str = tr("Cannot refine link %1->%2 (Intermediate node detected!)").arg(from).arg(to);
5811  }
5812  else
5813  {
5814  str = tr("Cannot refine link %1->%2 (scans empty!)").arg(from).arg(to);
5815  }
5816 
5817  _progressDialog->appendText(str, Qt::darkYellow);
5818  UWARN("%s", str.toStdString().c_str());
5819  warn = true;
5820  }
5821  }
5822  }
5823  }
5824  _progressDialog->appendText(tr("Refining links...done!"));
5825  }
5826 
5827  _progressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
5828  .arg(odomPoses.size()).arg(_currentLinksMap.size()));
5829 
5830  int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
5831  std::map<int, rtabmap::Transform> posesOut;
5832  std::multimap<int, rtabmap::Link> linksOut;
5833  std::map<int, rtabmap::Transform> optimizedPoses;
5834  optimizer->getConnectedGraph(
5835  fromId,
5836  odomPoses,
5837  _currentLinksMap,
5838  posesOut,
5839  linksOut);
5840  optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
5841  _progressDialog->appendText(tr("Optimizing graph with updated links... done!"));
5842  _progressDialog->incrementStep();
5843 
5844  if(!_progressCanceled && sba)
5845  {
5846  UASSERT(Optimizer::isAvailable(sbaType));
5847  _progressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
5848  .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
5849  QApplication::processEvents();
5850  uSleep(100);
5851  QApplication::processEvents();
5852 
5853  ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
5854  uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
5855  uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
5856  Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
5857  std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
5858  delete sba;
5859  if(newPoses.size())
5860  {
5861  optimizedPoses = newPoses;
5862  _progressDialog->appendText(tr("SBA... done!"));
5863  }
5864  else
5865  {
5866  _progressDialog->appendText(tr("SBA... failed!"));
5867  _progressDialog->setAutoClose(false);
5868  }
5869  _progressDialog->incrementStep();
5870  }
5871 
5872  _progressDialog->appendText(tr("Updating map..."));
5873  this->updateMapCloud(
5874  optimizedPoses,
5875  std::multimap<int, Link>(_currentLinksMap),
5876  std::map<int, int>(_currentMapIds),
5877  std::map<int, std::string>(_currentLabels),
5878  std::map<int, Transform>(_currentGTPosesMap),
5879  false);
5880  _progressDialog->appendText(tr("Updating map... done!"));
5881 
5882  if(warn)
5883  {
5884  _progressDialog->setAutoClose(false);
5885  }
5886 
5887  _progressDialog->setValue(_progressDialog->maximumSteps());
5888  _progressDialog->appendText("Post-processing finished!");
5889  _progressDialog->setCancelButtonVisible(false);
5890  _progressCanceled = false;
5891 
5892  delete optimizer;
5893 }
5894 
5895 void MainWindow::depthCalibration()
5896 {
5897  if(_currentPosesMap.size() && _cachedSignatures.size())
5898  {
5899  _depthCalibrationDialog->calibrate(
5900  _currentPosesMap,
5901  _cachedSignatures,
5902  _preferencesDialog->getWorkingDirectory(),
5903  _preferencesDialog->getAllParameters());
5904  }
5905  else
5906  {
5907  QMessageBox::warning(this, tr("Depth Calibration"), tr("No data in cache. Try to refresh the cache."));
5908  }
5909 }
5910 
5911 void MainWindow::deleteMemory()
5912 {
5913  QMessageBox::StandardButton button;
5914  if(_state == kMonitoring || _state == kMonitoringPaused)
5915  {
5916  button = QMessageBox::question(this,
5917  tr("Deleting memory..."),
5918  tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
5919  QMessageBox::Yes|QMessageBox::No,
5920  QMessageBox::No);
5921  }
5922  else
5923  {
5924  button = QMessageBox::question(this,
5925  tr("Deleting memory..."),
5926  tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
5927  QMessageBox::Yes|QMessageBox::No,
5928  QMessageBox::No);
5929  }
5930 
5931  if(button != QMessageBox::Yes)
5932  {
5933  return;
5934  }
5935 
5936  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
5937  if(_state!=kDetecting)
5938  {
5939  _databaseUpdated = false;
5940  }
5941  this->clearTheCache();
5942 }
5943 
5944 QString MainWindow::getWorkingDirectory() const
5945 {
5946  return _preferencesDialog->getWorkingDirectory();
5947 }
5948 
5949 void MainWindow::openWorkingDirectory()
5950 {
5951  QString filePath = _preferencesDialog->getWorkingDirectory();
5952 #if defined(Q_WS_MAC)
5953  QStringList args;
5954  args << "-e";
5955  args << "tell application \"Finder\"";
5956  args << "-e";
5957  args << "activate";
5958  args << "-e";
5959  args << "select POSIX file \""+filePath+"\"";
5960  args << "-e";
5961  args << "end tell";
5962  QProcess::startDetached("osascript", args);
5963 #elif defined(Q_WS_WIN)
5964  QStringList args;
5965  args << "/select," << QDir::toNativeSeparators(filePath);
5966  QProcess::startDetached("explorer", args);
5967 #else
5968  UERROR("Only works on Mac and Windows");
5969 #endif
5970 }
5971 
5972 void MainWindow::updateEditMenu()
5973 {
5974  // Update Memory delete database size
5975  if(_state != kMonitoring && _state != kMonitoringPaused && (!_openedDatabasePath.isEmpty() || !_newDatabasePath.isEmpty()))
5976  {
5977  if(!_openedDatabasePath.isEmpty())
5978  {
5979  _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_openedDatabasePath.toStdString())/1000000));
5980  }
5981  else
5982  {
5983  _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_newDatabasePath.toStdString())/1000000));
5984  }
5985  }
5986 }
5987 
5988 void MainWindow::selectStream()
5989 {
5990  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice);
5991 }
5992 
5993 void MainWindow::selectOpenni()
5994 {
5995  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
5996 }
5997 
5998 void MainWindow::selectFreenect()
5999 {
6000  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
6001 }
6002 
6003 void MainWindow::selectOpenniCv()
6004 {
6005  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
6006 }
6007 
6008 void MainWindow::selectOpenniCvAsus()
6009 {
6010  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
6011 }
6012 
6013 void MainWindow::selectOpenni2()
6014 {
6015  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
6016 }
6017 
6018 void MainWindow::selectFreenect2()
6019 {
6020  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
6021 }
6022 
6023 void MainWindow::selectK4W2()
6024 {
6025  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
6026 }
6027 
6028 void MainWindow::selectRealSense()
6029 {
6030  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);
6031 }
6032 
6033 void MainWindow::selectRealSense2()
6034 {
6035  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2);
6036 }
6037 
6038 void MainWindow::selectStereoDC1394()
6039 {
6040  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
6041 }
6042 
6043 void MainWindow::selectStereoFlyCapture2()
6044 {
6045  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
6046 }
6047 void MainWindow::selectStereoZed()
6048 {
6049  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed);
6050 }
6051 
6052 void MainWindow::selectStereoUsb()
6053 {
6054  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
6055 }
6056 
6057 void MainWindow::dumpTheMemory()
6058 {
6059  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
6060 }
6061 
6062 void MainWindow::dumpThePrediction()
6063 {
6064  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
6065 }
6066 
6067 void MainWindow::sendGoal()
6068 {
6069  UINFO("Sending a goal...");
6070  bool ok = false;
6071  QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
6072  if(ok && !text.isEmpty())
6073  {
6074  _waypoints.clear();
6075  _waypointsIndex = 0;
6076 
6077  this->postGoal(text);
6078  }
6079 }
6080 
6081 void MainWindow::sendWaypoints()
6082 {
6083  UINFO("Sending waypoints...");
6084  bool ok = false;
6085  QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
6086  if(ok && !text.isEmpty())
6087  {
6088  QStringList wp = text.split(' ');
6089  if(wp.size() < 2)
6090  {
6091  QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
6092  }
6093  else
6094  {
6095  _waypoints = wp;
6096  _waypointsIndex = 0;
6097  this->postGoal(_waypoints.at(_waypointsIndex));
6098  }
6099  }
6100 }
6101 
6102 void MainWindow::postGoal(const QString & goal)
6103 {
6104  if(!goal.isEmpty())
6105  {
6106  bool ok = false;
6107  int id = goal.toInt(&ok);
6108  _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
6109  UINFO("Posting event with goal %s", goal.toStdString().c_str());
6110  if(ok)
6111  {
6112  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
6113  }
6114  else
6115  {
6116  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
6117  }
6118  }
6119 }
6120 
6121 void MainWindow::cancelGoal()
6122 {
6123  UINFO("Cancelling goal...");
6124  _waypoints.clear();
6125  _waypointsIndex = 0;
6126  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
6127 }
6128 
6129 void MainWindow::label()
6130 {
6131  UINFO("Labelling current location...");
6132  bool ok = false;
6133  QString label = QInputDialog::getText(this, tr("Label current location"), tr("Label: "), QLineEdit::Normal, "", &ok);
6134  if(ok && !label.isEmpty())
6135  {
6136  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdLabel, label.toStdString(), 0));
6137  }
6138 }
6139 
6140 void MainWindow::updateCacheFromDatabase()
6141 {
6142  QString dir = getWorkingDirectory();
6143  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
6144  if(!path.isEmpty())
6145  {
6146  updateCacheFromDatabase(path);
6147  }
6148 }
6149 
6150 void MainWindow::updateCacheFromDatabase(const QString & path)
6151 {
6152  if(!path.isEmpty())
6153  {
6154  DBDriver * driver = DBDriver::create();
6155  if(driver->openConnection(path.toStdString()))
6156  {
6157  UINFO("Update cache...");
6158  _progressDialog->resetProgress();
6159  _progressDialog->show();
6160  _progressDialog->appendText(tr("Downloading the map from \"%1\" (without poses and links)...")
6161  .arg(path));
6162 
6163  std::set<int> ids;
6164  driver->getAllNodeIds(ids, true);
6165  std::list<Signature*> signaturesList;
6166  driver->loadSignatures(std::list<int>(ids.begin(), ids.end()), signaturesList);
6167  std::map<int, Signature> signatures;
6168  driver->loadNodeData(signaturesList);
6169  for(std::list<Signature *>::iterator iter=signaturesList.begin(); iter!=signaturesList.end(); ++iter)
6170  {
6171  signatures.insert(std::make_pair((*iter)->id(), *(*iter)));
6172  delete *iter;
6173  }
6174  RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap);
6175  processRtabmapEvent3DMap(event);
6176  }
6177  else
6178  {
6179  QMessageBox::warning(this, tr("Update cache"), tr("Failed to open database \"%1\"").arg(path));
6180  }
6181  delete driver;
6182  }
6183 }
6184 
6185 void MainWindow::downloadAllClouds()
6186 {
6187  QStringList items;
6188  items.append("Local map optimized");
6189  items.append("Local map not optimized");
6190  items.append("Global map optimized");
6191  items.append("Global map not optimized");
6192 
6193  bool ok;
6194  QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 2, false, &ok);
6195  if(ok)
6196  {
6197  bool optimized=false, global=false;
6198  if(item.compare("Local map optimized") == 0)
6199  {
6200  optimized = true;
6201  }
6202  else if(item.compare("Local map not optimized") == 0)
6203  {
6204 
6205  }
6206  else if(item.compare("Global map optimized") == 0)
6207  {
6208  global=true;
6209  optimized=true;
6210  }
6211  else if(item.compare("Global map not optimized") == 0)
6212  {
6213  global=true;
6214  }
6215  else
6216  {
6217  UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
6218  }
6219 
6220  UINFO("Download clouds...");
6221  _progressDialog->resetProgress();
6222  _progressDialog->show();
6223  _progressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
6224  .arg(global?"true":"false").arg(optimized?"true":"false"));
6225  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
6226  }
6227 }
6228 
6229 void MainWindow::downloadPoseGraph()
6230 {
6231  QStringList items;
6232  items.append("Local map optimized");
6233  items.append("Local map not optimized");
6234  items.append("Global map optimized");
6235  items.append("Global map not optimized");
6236 
6237  bool ok;
6238  QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 2, false, &ok);
6239  if(ok)
6240  {
6241  bool optimized=false, global=false;
6242  if(item.compare("Local map optimized") == 0)
6243  {
6244  optimized = true;
6245  }
6246  else if(item.compare("Local map not optimized") == 0)
6247  {
6248 
6249  }
6250  else if(item.compare("Global map optimized") == 0)
6251  {
6252  global=true;
6253  optimized=true;
6254  }
6255  else if(item.compare("Global map not optimized") == 0)
6256  {
6257  global=true;
6258  }
6259  else
6260  {
6261  UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
6262  }
6263 
6264  UINFO("Download the graph...");
6265  _progressDialog->resetProgress();
6266  _progressDialog->show();
6267  _progressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
6268  .arg(global?"true":"false").arg(optimized?"true":"false"));
6269 
6270  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
6271  }
6272 }
6273 
6274 void MainWindow::anchorCloudsToGroundTruth()
6275 {
6276  this->updateMapCloud(
6277  std::map<int, Transform>(_currentPosesMap),
6278  std::multimap<int, Link>(_currentLinksMap),
6279  std::map<int, int>(_currentMapIds),
6280  std::map<int, std::string>(_currentLabels),
6281  std::map<int, Transform>(_currentGTPosesMap));
6282 }
6283 
6284 void MainWindow::clearTheCache()
6285 {
6286  _cachedSignatures.clear();
6287  _cachedMemoryUsage = 0;
6288  _cachedWordsCount.clear();
6289  _cachedClouds.clear();
6290  _createdCloudsMemoryUsage = 0;
6291  _cachedEmptyClouds.clear();
6292  _previousCloud.first = 0;
6293  _previousCloud.second.first.first.reset();
6294  _previousCloud.second.first.second.reset();
6295  _previousCloud.second.second.reset();
6296  _createdScans.clear();
6297  _createdFeatures.clear();
6298  _cloudViewer->clear();
6299  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
6300  _cloudViewer->clearTrajectory();
6301  _ui->widget_mapVisibility->clear();
6302  _currentPosesMap.clear();
6303  _currentGTPosesMap.clear();
6304  _currentLinksMap.clear();
6305  _currentMapIds.clear();
6306  _currentLabels.clear();
6307  _odometryCorrection = Transform::getIdentity();
6308  _lastOdomPose.setNull();
6309  _ui->statsToolBox->clear();
6310  //disable save cloud action
6311  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
6312  _ui->actionPost_processing->setEnabled(false);
6313  _ui->actionSave_point_cloud->setEnabled(false);
6314  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
6315  _ui->actionDepth_Calibration->setEnabled(false);
6316  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
6317  _ui->actionExport_octomap->setEnabled(false);
6318  _ui->actionView_high_res_point_cloud->setEnabled(false);
6319  _likelihoodCurve->clear();
6320  _rawLikelihoodCurve->clear();
6321  _posteriorCurve->clear();
6322  _lastId = 0;
6323  _lastIds.clear();
6324  _firstStamp = 0.0f;
6325  _ui->label_stats_loopClosuresDetected->setText("0");
6326  _ui->label_stats_loopClosuresReactivatedDetected->setText("0");
6327  _ui->label_stats_loopClosuresRejected->setText("0");
6328  _refIds.clear();
6329  _loopClosureIds.clear();
6330  _cachedLocalizationsCount.clear();
6331  _ui->label_refId->clear();
6332  _ui->label_matchId->clear();
6333  _ui->graphicsView_graphView->clearAll();
6334  _ui->imageView_source->clear();
6335  _ui->imageView_loopClosure->clear();
6336  _ui->imageView_odometry->clear();
6337  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
6338  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
6339  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
6340 #ifdef RTABMAP_OCTOMAP
6341  // re-create one if the resolution has changed
6342  UASSERT(_octomap != 0);
6343  delete _octomap;
6344  _octomap = new OctoMap(_preferencesDialog->getAllParameters());
6345 #endif
6346  _occupancyGrid->clear();
6347  _rectCameraModels.clear();
6348 }
6349 
6350 void MainWindow::openHelp()
6351 {
6352  if(_state == kMonitoringPaused || _state == kMonitoring)
6353  {
6354  QDesktopServices::openUrl(QUrl("http://wiki.ros.org/rtabmap_ros"));
6355  }
6356  else
6357  {
6358  QDesktopServices::openUrl(QUrl("https://github.com/introlab/rtabmap/wiki"));
6359  }
6360 }
6361 
6362 void MainWindow::updateElapsedTime()
6363 {
6364  if(_state == kDetecting || _state == kMonitoring)
6365  {
6366  QString format = "hh:mm:ss";
6367  _ui->label_elapsedTime->setText((QTime().fromString(_ui->label_elapsedTime->text(), format).addMSecs(_elapsedTime->restart())).toString(format));
6368  }
6369 }
6370 
6371 void MainWindow::saveFigures()
6372 {
6373  QList<int> curvesPerFigure;
6374  QStringList curveNames;
6375  _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
6376 
6377  QStringList curvesPerFigureStr;
6378  for(int i=0; i<curvesPerFigure.size(); ++i)
6379  {
6380  curvesPerFigureStr.append(QString::number(curvesPerFigure[i]));
6381  }
6382  for(int i=0; i<curveNames.size(); ++i)
6383  {
6384  curveNames[i].replace(' ', '_');
6385  }
6386  _preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
6387  _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
6388 }
6389 
6390 void MainWindow::loadFigures()
6391 {
6392  QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
6393  QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
6394 
6395  if(!curvesPerFigure.isEmpty())
6396  {
6397  QStringList curvesPerFigureList = curvesPerFigure.split(" ");
6398  QStringList curvesNamesList = curveNames.split(" ");
6399 
6400  int j=0;
6401  for(int i=0; i<curvesPerFigureList.size(); ++i)
6402  {
6403  bool ok = false;
6404  int count = curvesPerFigureList[i].toInt(&ok);
6405  if(!ok)
6406  {
6407  QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
6408  break;
6409  }
6410  else
6411  {
6412  _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
6413  for(int k=1; k<count && j<curveNames.size(); ++k)
6414  {
6415  _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
6416  }
6417  }
6418  }
6419  }
6420 
6421 }
6422 
6423 void MainWindow::openPreferences()
6424 {
6425  _preferencesDialog->setMonitoringState(_state == kMonitoring || _state == kMonitoringPaused);
6426  _preferencesDialog->exec();
6427 }
6428 
6429 void MainWindow::openPreferencesSource()
6430 {
6431  _preferencesDialog->setCurrentPanelToSource();
6432  openPreferences();
6433  this->updateSelectSourceMenu();
6434 }
6435 
6436 void MainWindow::setDefaultViews()
6437 {
6438  _ui->dockWidget_posterior->setVisible(false);
6439  _ui->dockWidget_likelihood->setVisible(false);
6440  _ui->dockWidget_rawlikelihood->setVisible(false);
6441  _ui->dockWidget_statsV2->setVisible(false);
6442  _ui->dockWidget_console->setVisible(false);
6443  _ui->dockWidget_loopClosureViewer->setVisible(false);
6444  _ui->dockWidget_mapVisibility->setVisible(false);
6445  _ui->dockWidget_graphViewer->setVisible(false);
6446  _ui->dockWidget_odometry->setVisible(true);
6447  _ui->dockWidget_cloudViewer->setVisible(true);
6448  _ui->dockWidget_imageView->setVisible(true);
6449  _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused);
6450  _ui->toolBar_2->setVisible(true);
6451  _ui->statusbar->setVisible(false);
6452  this->setAspectRatio720p();
6453  _cloudViewer->resetCamera();
6454  _cloudViewer->setCameraLockZ(true);
6455  _cloudViewer->setCameraTargetFollow(true);
6456 }
6457 
6458 void MainWindow::selectScreenCaptureFormat(bool checked)
6459 {
6460  if(checked)
6461  {
6462  QStringList items;
6463  items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
6464  bool ok;
6465  QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
6466  if(ok && !item.isEmpty())
6467  {
6468  if(item.compare("Synchronize with map update") == 0)
6469  {
6470  _autoScreenCaptureOdomSync = false;
6471  }
6472  else
6473  {
6474  _autoScreenCaptureOdomSync = true;
6475  }
6476 
6477  if(_state != kMonitoring && _state != kMonitoringPaused)
6478  {
6479  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);
6480  if(r == QMessageBox::No || r == QMessageBox::Yes)
6481  {
6482  _autoScreenCaptureRAM = r == QMessageBox::Yes;
6483  }
6484  else
6485  {
6486  _ui->actionAuto_screen_capture->setChecked(false);
6487  }
6488 
6489  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);
6490  if(r == QMessageBox::No || r == QMessageBox::Yes)
6491  {
6492  _autoScreenCapturePNG = r == QMessageBox::No;
6493  }
6494  else
6495  {
6496  _ui->actionAuto_screen_capture->setChecked(false);
6497  }
6498  }
6499  }
6500  else
6501  {
6502  _ui->actionAuto_screen_capture->setChecked(false);
6503  }
6504  }
6505  else if(_autoScreenCaptureCachedImages.size())
6506  {
6507  QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
6508  QDir dir;
6509  if(!dir.exists(targetDir))
6510  {
6511  dir.mkdir(targetDir);
6512  }
6513  targetDir += QDir::separator();
6514  targetDir += "Main_window";
6515  if(!dir.exists(targetDir))
6516  {
6517  dir.mkdir(targetDir);
6518  }
6519  targetDir += QDir::separator();
6520 
6521  _progressDialog->setCancelButtonVisible(true);
6522  _progressDialog->resetProgress();
6523  _progressDialog->show();
6524  _progressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size());
6525  int i=0;
6526  for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end() && !_progressDialog->isCanceled(); ++iter)
6527  {
6528  QPixmap figure;
6529  figure.loadFromData(iter.value(), _autoScreenCapturePNG?"PNG":"JPEG");
6530  figure.save(targetDir + iter.key(), _autoScreenCapturePNG?"PNG":"JPEG");
6531  _progressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size()));
6532  _progressDialog->incrementStep();
6533  QApplication::processEvents();
6534  }
6535  _autoScreenCaptureCachedImages.clear();
6536  _progressDialog->setValue(_progressDialog->maximumSteps());
6537  _progressDialog->setCancelButtonVisible(false);
6538  }
6539 }
6540 
6541 void MainWindow::takeScreenshot()
6542 {
6543  QDesktopServices::openUrl(QUrl::fromLocalFile(this->captureScreen()));
6544 }
6545 
6546 void MainWindow::setAspectRatio(int w, int h)
6547 {
6548  QRect rect = this->geometry();
6549  if(h<100 && w<100)
6550  {
6551  // it is a ratio
6552  if(float(rect.width())/float(rect.height()) > float(w)/float(h))
6553  {
6554  rect.setWidth(w*(rect.height()/h));
6555  rect.setHeight((rect.height()/h)*h);
6556  }
6557  else
6558  {
6559  rect.setHeight(h*(rect.width()/w));
6560  rect.setWidth((rect.width()/w)*w);
6561  }
6562  }
6563  else
6564  {
6565  // it is absolute size
6566  rect.setWidth(w);
6567  rect.setHeight(h);
6568  }
6569  this->setGeometry(rect);
6570 }
6571 
6572 void MainWindow::setAspectRatio16_9()
6573 {
6574  this->setAspectRatio(16, 9);
6575 }
6576 
6577 void MainWindow::setAspectRatio16_10()
6578 {
6579  this->setAspectRatio(16, 10);
6580 }
6581 
6582 void MainWindow::setAspectRatio4_3()
6583 {
6584  this->setAspectRatio(4, 3);
6585 }
6586 
6587 void MainWindow::setAspectRatio240p()
6588 {
6589  this->setAspectRatio((240*16)/9, 240);
6590 }
6591 
6592 void MainWindow::setAspectRatio360p()
6593 {
6594  this->setAspectRatio((360*16)/9, 360);
6595 }
6596 
6597 void MainWindow::setAspectRatio480p()
6598 {
6599  this->setAspectRatio((480*16)/9, 480);
6600 }
6601 
6602 void MainWindow::setAspectRatio720p()
6603 {
6604  this->setAspectRatio((720*16)/9, 720);
6605 }
6606 
6607 void MainWindow::setAspectRatio1080p()
6608 {
6609  this->setAspectRatio((1080*16)/9, 1080);
6610 }
6611 
6612 void MainWindow::setAspectRatioCustom()
6613 {
6614  bool ok;
6615  int width = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Width (pixels):"), this->geometry().width(), 100, 10000, 100, &ok);
6616  if(ok)
6617  {
6618  int height = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Height (pixels):"), this->geometry().height(), 100, 10000, 100, &ok);
6619  if(ok)
6620  {
6621  this->setAspectRatio(width, height);
6622  }
6623  }
6624 }
6625 
6626 void MainWindow::exportGridMap()
6627 {
6628  float gridCellSize = 0.05f;
6629  bool ok;
6630  gridCellSize = (float)QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), (double)gridCellSize, 0.01, 1, 2, &ok);
6631  if(!ok)
6632  {
6633  return;
6634  }
6635 
6636  // create the map
6637  float xMin=0.0f, yMin=0.0f;
6638  cv::Mat pixels;
6639 #ifdef RTABMAP_OCTOMAP
6640  if(_preferencesDialog->isOctomap2dGrid())
6641  {
6642  pixels = _octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
6643 
6644  }
6645  else
6646 #endif
6647  {
6648  pixels = _occupancyGrid->getMap(xMin, yMin);
6649  }
6650 
6651  if(!pixels.empty())
6652  {
6653  cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
6654  //convert to gray scaled map
6655  for (int i = 0; i < pixels.rows; ++i)
6656  {
6657  for (int j = 0; j < pixels.cols; ++j)
6658  {
6659  char v = pixels.at<char>(i, j);
6660  unsigned char gray;
6661  if(v == 0)
6662  {
6663  gray = 178;
6664  }
6665  else if(v == 100)
6666  {
6667  gray = 0;
6668  }
6669  else // -1
6670  {
6671  gray = 89;
6672  }
6673  map8U.at<unsigned char>(i, j) = gray;
6674  }
6675  }
6676 
6677  QImage image = uCvMat2QImage(map8U, false);
6678 
6679  QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
6680  if(!path.isEmpty())
6681  {
6682  if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
6683  {
6684  //use png by default
6685  path += ".png";
6686  }
6687 
6688  QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
6689  QPixmap::fromImage(img).save(path);
6690 
6691  QDesktopServices::openUrl(QUrl::fromLocalFile(path));
6692  }
6693  }
6694 }
6695 
6696 void MainWindow::exportClouds()
6697 {
6698  if(_exportCloudsDialog->isVisible())
6699  {
6700  return;
6701  }
6702 
6703  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
6704 
6705  // Use ground truth poses if current clouds are using them
6706  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
6707  {
6708  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
6709  {
6710  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
6711  if(gtIter!=_currentGTPosesMap.end())
6712  {
6713  iter->second = gtIter->second;
6714  }
6715  else
6716  {
6717  UWARN("Not found ground truth pose for node %d", iter->first);
6718  }
6719  }
6720  }
6721 
6722  _exportCloudsDialog->exportClouds(
6723  poses,
6724  _currentLinksMap,
6725  _currentMapIds,
6726  _cachedSignatures,
6727  _cachedClouds,
6728  _createdScans,
6729  _preferencesDialog->getWorkingDirectory(),
6730  _preferencesDialog->getAllParameters());
6731 }
6732 
6733 void MainWindow::viewClouds()
6734 {
6735  if(_exportCloudsDialog->isVisible())
6736  {
6737  return;
6738  }
6739 
6740  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
6741 
6742  // Use ground truth poses if current clouds are using them
6743  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
6744  {
6745  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
6746  {
6747  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
6748  if(gtIter!=_currentGTPosesMap.end())
6749  {
6750  iter->second = gtIter->second;
6751  }
6752  else
6753  {
6754  UWARN("Not found ground truth pose for node %d", iter->first);
6755  }
6756  }
6757  }
6758 
6759  _exportCloudsDialog->viewClouds(
6760  poses,
6761  _currentLinksMap,
6762  _currentMapIds,
6763  _cachedSignatures,
6764  _cachedClouds,
6765  _createdScans,
6766  _preferencesDialog->getWorkingDirectory(),
6767  _preferencesDialog->getAllParameters());
6768 
6769 }
6770 
6771 void MainWindow::exportOctomap()
6772 {
6773 #ifdef RTABMAP_OCTOMAP
6774  if(_octomap->octree()->size())
6775  {
6776  QString path = QFileDialog::getSaveFileName(
6777  this,
6778  tr("Save File"),
6779  this->getWorkingDirectory()+"/"+"octomap.bt",
6780  tr("Octomap file (*.bt)"));
6781 
6782  if(!path.isEmpty())
6783  {
6784  if(_octomap->writeBinary(path.toStdString()))
6785  {
6786  QMessageBox::information(this,
6787  tr("Export octomap..."),
6788  tr("Octomap successfully saved to \"%1\".")
6789  .arg(path));
6790  }
6791  else
6792  {
6793  QMessageBox::information(this,
6794  tr("Export octomap..."),
6795  tr("Failed to save octomap to \"%1\"!")
6796  .arg(path));
6797  }
6798  }
6799  }
6800  else
6801  {
6802  UERROR("Empty octomap.");
6803  }
6804 #else
6805  UERROR("Cannot export octomap, RTAB-Map is not built with it.");
6806 #endif
6807 }
6808 
6809 void MainWindow::exportImages()
6810 {
6811  if(_cachedSignatures.empty())
6812  {
6813  QMessageBox::warning(this, tr("Export images..."), tr("Cannot export images, the cache is empty!"));
6814  return;
6815  }
6816  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
6817 
6818  if(poses.empty())
6819  {
6820  QMessageBox::warning(this, tr("Export images..."), tr("There is no map!"));
6821  return;
6822  }
6823 
6824  QStringList formats;
6825  formats.push_back("jpg");
6826  formats.push_back("png");
6827  bool ok;
6828  QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
6829  if(!ok)
6830  {
6831  return;
6832  }
6833 
6834  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), this->getWorkingDirectory());
6835  if(!path.isEmpty())
6836  {
6837  SensorData data;
6838  if(_cachedSignatures.contains(poses.rbegin()->first))
6839  {
6840  data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
6841  data.uncompressData();
6842  }
6843  if(!data.imageRaw().empty() && !data.rightRaw().empty())
6844  {
6845  QDir dir;
6846  dir.mkdir(QString("%1/left").arg(path));
6847  dir.mkdir(QString("%1/right").arg(path));
6848  if(data.stereoCameraModel().isValidForProjection())
6849  {
6850  std::string cameraName = "calibration";
6851  StereoCameraModel model(
6852  cameraName,
6853  data.imageRaw().size(),
6854  data.stereoCameraModel().left().K(),
6855  data.stereoCameraModel().left().D(),
6856  data.stereoCameraModel().left().R(),
6857  data.stereoCameraModel().left().P(),
6858  data.rightRaw().size(),
6859  data.stereoCameraModel().right().K(),
6860  data.stereoCameraModel().right().D(),
6861  data.stereoCameraModel().right().R(),
6862  data.stereoCameraModel().right().P(),
6863  data.stereoCameraModel().R(),
6864  data.stereoCameraModel().T(),
6865  data.stereoCameraModel().E(),
6866  data.stereoCameraModel().F(),
6867  data.stereoCameraModel().left().localTransform());
6868  if(model.save(path.toStdString()))
6869  {
6870  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
6871  }
6872  else
6873  {
6874  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
6875  }
6876  }
6877  }
6878  else if(!data.imageRaw().empty())
6879  {
6880  if(!data.depthRaw().empty())
6881  {
6882  QDir dir;
6883  dir.mkdir(QString("%1/rgb").arg(path));
6884  dir.mkdir(QString("%1/depth").arg(path));
6885  }
6886 
6887  if(data.cameraModels().size() > 1)
6888  {
6889  UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
6890  }
6891  else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
6892  {
6893  std::string cameraName = "calibration";
6894  CameraModel model(cameraName,
6895  data.imageRaw().size(),
6896  data.cameraModels().front().K(),
6897  data.cameraModels().front().D(),
6898  data.cameraModels().front().R(),
6899  data.cameraModels().front().P(),
6900  data.cameraModels().front().localTransform());
6901  if(model.save(path.toStdString()))
6902  {
6903  UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
6904  }
6905  else
6906  {
6907  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
6908  }
6909  }
6910  }
6911  else
6912  {
6913  QMessageBox::warning(this,
6914  tr("Export images..."),
6915  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));
6916  }
6917 
6918  _progressDialog->resetProgress();
6919  _progressDialog->show();
6920  _progressDialog->setMaximumSteps(_cachedSignatures.size());
6921 
6922  unsigned int saved = 0;
6923  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
6924  {
6925  int id = iter->first;
6926  SensorData data;
6927  if(_cachedSignatures.contains(iter->first))
6928  {
6929  data = _cachedSignatures.value(iter->first).sensorData();
6930  data.uncompressData();
6931  }
6932  QString info;
6933  bool warn = false;
6934  if(!data.imageRaw().empty() && !data.rightRaw().empty())
6935  {
6936  cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
6937  cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
6938  info = tr("Saved left/%1.%2 and right/%1.%2.").arg(id).arg(ext);
6939  }
6940  else if(!data.imageRaw().empty() && !data.depthRaw().empty())
6941  {
6942  cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
6943  cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
6944  info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
6945  }
6946  else if(!data.imageRaw().empty())
6947  {
6948  cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
6949  info = tr("Saved %1.%2.").arg(id).arg(ext);
6950  }
6951  else
6952  {
6953  info = tr("No images saved for node %1!").arg(id);
6954  warn = true;
6955  }
6956  saved += warn?0:1;
6957  _progressDialog->appendText(info, !warn?Qt::black:Qt::darkYellow);
6958  _progressDialog->incrementStep();
6959  QApplication::processEvents();
6960 
6961  }
6962  if(saved!=poses.size())
6963  {
6964  _progressDialog->setAutoClose(false);
6965  _progressDialog->appendText(tr("%1 images of %2 saved to \"%3\".").arg(saved).arg(poses.size()).arg(path));
6966  }
6967  else
6968  {
6969  _progressDialog->appendText(tr("%1 images saved to \"%2\".").arg(saved).arg(path));
6970  }
6971 
6972  _progressDialog->setValue(_progressDialog->maximumSteps());
6973  }
6974 }
6975 
6976 void MainWindow::exportBundlerFormat()
6977 {
6978  if(_exportBundlerDialog->isVisible())
6979  {
6980  return;
6981  }
6982 
6983  std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
6984 
6985  // Use ground truth poses if current clouds are using them
6986  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
6987  {
6988  for(std::map<int, Transform>::iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
6989  {
6990  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
6991  if(gtIter!=_currentGTPosesMap.end())
6992  {
6993  iter->second = gtIter->second;
6994  }
6995  else
6996  {
6997  UWARN("Not found ground truth pose for node %d", iter->first);
6998  }
6999  }
7000  }
7001 
7002  std::map<int, Transform> poses;
7003  for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
7004  {
7005  if(_cachedSignatures.contains(iter->first))
7006  {
7007  if(_cachedSignatures[iter->first].sensorData().imageRaw().empty() &&
7008  _cachedSignatures[iter->first].sensorData().imageCompressed().empty())
7009  {
7010  UWARN("Missing image in cache for node %d", iter->first);
7011  }
7012  else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
7013  _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection())
7014  {
7015  poses.insert(*iter);
7016  }
7017  else
7018  {
7019  UWARN("Missing calibration for node %d", iter->first);
7020  }
7021  }
7022  else
7023  {
7024  UWARN("Did not find node %d in cache", iter->first);
7025  }
7026  }
7027 
7028  if(poses.size())
7029  {
7030  if(_exportBundlerDialog->exec() != QDialog::Accepted)
7031  {
7032  return;
7033  }
7034  QString path = _exportBundlerDialog->outputPath();
7035  if(!path.isEmpty())
7036  {
7037  if(!QDir(path).mkpath("."))
7038  {
7039  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
7040  return;
7041  }
7042  // export cameras and images
7043  QFile fileOut(path+QDir::separator()+"cameras.out");
7044  QFile fileList(path+QDir::separator()+"list.txt");
7045  QDir(path).mkdir("images");
7046  if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
7047  {
7048  if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
7049  {
7050  std::set<int> ignoredCameras;
7051  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
7052  {
7053  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
7054  p = path+QDir::separator()+p;
7055  cv::Mat image = _cachedSignatures[iter->first].sensorData().imageRaw();
7056  if(image.empty())
7057  {
7058  _cachedSignatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
7059  }
7060 
7061  double maxLinearVel = _exportBundlerDialog->maxLinearSpeed();
7062  double maxAngularVel = _exportBundlerDialog->maxAngularSpeed();
7063  double laplacianThr = _exportBundlerDialog->laplacianThreshold();
7064  bool blurryImage = false;
7065  const std::vector<float> & velocity = _cachedSignatures[iter->first].getVelocity();
7066  if(maxLinearVel>0.0 || maxAngularVel>0.0)
7067  {
7068  if(velocity.size() == 6)
7069  {
7070  float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
7071  float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
7072  if(maxLinearVel>0.0 && transVel > maxLinearVel)
7073  {
7074  UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
7075  blurryImage = true;
7076  }
7077  else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
7078  {
7079  UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
7080  blurryImage = true;
7081  }
7082  }
7083  else
7084  {
7085  UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
7086  }
7087  }
7088 
7089  if(!blurryImage && !image.empty() && laplacianThr>0.0)
7090  {
7091  cv::Mat imgLaplacian;
7092  cv::Laplacian(image, imgLaplacian, CV_16S);
7093  cv::Mat m, s;
7094  cv::meanStdDev(imgLaplacian, m, s);
7095  double stddev_pxl = s.at<double>(0);
7096  double var = stddev_pxl*stddev_pxl;
7097  if(var < laplacianThr)
7098  {
7099  blurryImage = true;
7100  UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
7101  }
7102  }
7103  if(blurryImage)
7104  {
7105  ignoredCameras.insert(iter->first);
7106  }
7107  else
7108  {
7109  if(cv::imwrite(p.toStdString(), image))
7110  {
7111  UINFO("saved image %s", p.toStdString().c_str());
7112  }
7113  else
7114  {
7115  UERROR("Failed to save image %s", p.toStdString().c_str());
7116  }
7117  }
7118  }
7119 
7120  QTextStream out(&fileOut);
7121  QTextStream list(&fileList);
7122  out << "# Bundle file v0.3\n";
7123  out << poses.size()-ignoredCameras.size() << " 0\n";
7124 
7125  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
7126  {
7127  if(ignoredCameras.find(iter->first) == ignoredCameras.end())
7128  {
7129  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
7130  list << p << "\n";
7131 
7132  Transform localTransform;
7133  if(_cachedSignatures[iter->first].sensorData().cameraModels().size())
7134  {
7135  out << _cachedSignatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
7136  localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
7137  }
7138  else
7139  {
7140  out << _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
7141  localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
7142  }
7143 
7144  static const Transform opengl_world_T_rtabmap_world(
7145  0.0f, -1.0f, 0.0f, 0.0f,
7146  0.0f, 0.0f, 1.0f, 0.0f,
7147  -1.0f, 0.0f, 0.0f, 0.0f);
7148 
7149  static const Transform optical_rotation_inv(
7150  0.0f, -1.0f, 0.0f, 0.0f,
7151  0.0f, 0.0f, -1.0f, 0.0f,
7152  1.0f, 0.0f, 0.0f, 0.0f);
7153 
7154  Transform pose = iter->second;
7155  if(!localTransform.isNull())
7156  {
7157  pose*=localTransform*optical_rotation_inv;
7158  }
7159  Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
7160 
7161  out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
7162  out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
7163  out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
7164  out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
7165 
7166  }
7167  }
7168 
7169  fileList.close();
7170  fileOut.close();
7171 
7172  QMessageBox::information(this,
7173  tr("Exporting cameras in Bundler format..."),
7174  tr("%1 cameras/images exported to directory \"%2\".%3")
7175  .arg(poses.size())
7176  .arg(path)
7177  .arg(ignoredCameras.size()>0?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(ignoredCameras.size()).arg(poses.size()):""));
7178  }
7179  else
7180  {
7181  fileOut.close();
7182  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
7183  }
7184  }
7185  else
7186  {
7187  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
7188  }
7189  }
7190  }
7191  else
7192  {
7193  QMessageBox::warning(this, tr("Exporting cameras..."), tr("No poses exported because of missing images. Try refreshing the cache (with clouds)."));
7194  }
7195 }
7196 
7197 void MainWindow::resetOdometry()
7198 {
7199  UINFO("reset odometry");
7200  this->post(new OdometryResetEvent());
7201 }
7202 
7203 void MainWindow::triggerNewMap()
7204 {
7205  UINFO("trigger a new map");
7206  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
7207 }
7208 
7209 void MainWindow::dataRecorder()
7210 {
7211  if(_dataRecorder == 0)
7212  {
7213  QString path = QFileDialog::getSaveFileName(this, tr("Save to..."), _preferencesDialog->getWorkingDirectory()+"/output.db", "RTAB-Map database (*.db)");
7214  if(!path.isEmpty())
7215  {
7216  int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
7217 
7218  if(r == QMessageBox::No || r == QMessageBox::Yes)
7219  {
7220  bool recordInRAM = r == QMessageBox::Yes;
7221 
7222  _dataRecorder = new DataRecorder(this);
7223  _dataRecorder->setWindowFlags(Qt::Dialog);
7224  _dataRecorder->setAttribute(Qt::WA_DeleteOnClose, true);
7225  _dataRecorder->setWindowTitle(tr("Data recorder (%1)").arg(path));
7226 
7227  if(_dataRecorder->init(path, recordInRAM))
7228  {
7229  this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed()));
7230  _dataRecorder->show();
7231  _dataRecorder->registerToEventsManager();
7232  if(_camera)
7233  {
7234  UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
7235  }
7236  _ui->actionData_recorder->setEnabled(false);
7237  }
7238  else
7239  {
7240  QMessageBox::warning(this, tr(""), tr("Cannot initialize the data recorder!"));
7241  UERROR("Cannot initialize the data recorder!");
7242  delete _dataRecorder;
7243  _dataRecorder = 0;
7244  }
7245  }
7246  }
7247  }
7248  else
7249  {
7250  UERROR("Only one recorder at the same time.");
7251  }
7252 }
7253 
7254 void MainWindow::dataRecorderDestroyed()
7255 {
7256  _ui->actionData_recorder->setEnabled(true);
7257  _dataRecorder = 0;
7258 }
7259 
7260 //END ACTIONS
7261 
7262 // STATES
7263 
7264 // in monitoring state, only some actions are enabled
7265 void MainWindow::setMonitoringState(bool pauseChecked)
7266 {
7267  this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
7268 }
7269 
7270 // Must be called by the GUI thread, use signal StateChanged()
7271 void MainWindow::changeState(MainWindow::State newState)
7272 {
7273  bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
7274  _ui->label_source->setVisible(!monitoring);
7275  _ui->label_stats_source->setVisible(!monitoring);
7276  _ui->actionNew_database->setVisible(!monitoring);
7277  _ui->actionOpen_database->setVisible(!monitoring);
7278  _ui->actionClose_database->setVisible(!monitoring);
7279  _ui->actionEdit_database->setVisible(!monitoring);
7280  _ui->actionStart->setVisible(!monitoring);
7281  _ui->actionStop->setVisible(!monitoring);
7282  _ui->actionDump_the_memory->setVisible(!monitoring);
7283  _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
7284  _ui->actionGenerate_map->setVisible(!monitoring);
7285  _ui->actionUpdate_cache_from_database->setVisible(monitoring);
7286  _ui->actionData_recorder->setVisible(!monitoring);
7287  _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
7288  _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
7289  _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
7290  bool wasMonitoring = _state==kMonitoring || _state == kMonitoringPaused;
7291  if(wasMonitoring != monitoring)
7292  {
7293  _ui->toolBar->setVisible(!monitoring);
7294  _ui->toolBar->toggleViewAction()->setVisible(!monitoring);
7295  }
7296  QList<QAction*> actions = _ui->menuTools->actions();
7297  for(int i=0; i<actions.size(); ++i)
7298  {
7299  if(actions.at(i)->isSeparator())
7300  {
7301  actions.at(i)->setVisible(!monitoring);
7302  }
7303  }
7304  actions = _ui->menuFile->actions();
7305  if(actions.size()==16)
7306  {
7307  if(actions.at(2)->isSeparator())
7308  {
7309  actions.at(2)->setVisible(!monitoring);
7310  }
7311  else
7312  {
7313  UWARN("Menu File separators have not the same order.");
7314  }
7315  if(actions.at(12)->isSeparator())
7316  {
7317  actions.at(12)->setVisible(!monitoring);
7318  }
7319  else
7320  {
7321  UWARN("Menu File separators have not the same order.");
7322  }
7323  }
7324  else
7325  {
7326  UWARN("Menu File actions size has changed (%d)", actions.size());
7327  }
7328  actions = _ui->menuProcess->actions();
7329  if(actions.size()>=2)
7330  {
7331  if(actions.at(1)->isSeparator())
7332  {
7333  actions.at(1)->setVisible(!monitoring);
7334  }
7335  else
7336  {
7337  UWARN("Menu File separators have not the same order.");
7338  }
7339  }
7340  else
7341  {
7342  UWARN("Menu File separators have not the same order.");
7343  }
7344 
7345  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
7346 
7347  switch (newState)
7348  {
7349  case kIdle: // RTAB-Map is not initialized yet
7350  _ui->actionNew_database->setEnabled(true);
7351  _ui->actionOpen_database->setEnabled(true);
7352  _ui->actionClose_database->setEnabled(false);
7353  _ui->actionEdit_database->setEnabled(true);
7354  _ui->actionStart->setEnabled(false);
7355  _ui->actionPause->setEnabled(false);
7356  _ui->actionPause->setChecked(false);
7357  _ui->actionPause->setToolTip(tr("Pause"));
7358  _ui->actionStop->setEnabled(false);
7359  _ui->actionPause_on_match->setEnabled(true);
7360  _ui->actionPause_on_local_loop_detection->setEnabled(true);
7361  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
7362  _ui->actionDump_the_memory->setEnabled(false);
7363  _ui->actionDump_the_prediction_matrix->setEnabled(false);
7364  _ui->actionDelete_memory->setEnabled(false);
7365  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
7366  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7367  _ui->actionGenerate_map->setEnabled(false);
7368  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
7369  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7370  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7371  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
7372 #ifdef RTABMAP_OCTOMAP
7373  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
7374 #else
7375  _ui->actionExport_octomap->setEnabled(false);
7376 #endif
7377  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7378  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7379  _ui->actionDownload_all_clouds->setEnabled(false);
7380  _ui->actionDownload_graph->setEnabled(false);
7381  _ui->menuSelect_source->setEnabled(true);
7382  _ui->actionLabel_current_location->setEnabled(false);
7383  _ui->actionSend_goal->setEnabled(false);
7384  _ui->actionCancel_goal->setEnabled(false);
7385  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
7386  _ui->actionTrigger_a_new_map->setEnabled(false);
7387  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
7388  _ui->statusbar->clearMessage();
7389  _state = newState;
7390  _oneSecondTimer->stop();
7391  break;
7392 
7393  case kApplicationClosing:
7394  case kClosing:
7395  _ui->actionStart->setEnabled(false);
7396  _ui->actionPause->setEnabled(false);
7397  _ui->actionStop->setEnabled(false);
7398  _state = newState;
7399  break;
7400 
7401  case kInitializing:
7402  _ui->actionNew_database->setEnabled(false);
7403  _ui->actionOpen_database->setEnabled(false);
7404  _ui->actionClose_database->setEnabled(false);
7405  _ui->actionEdit_database->setEnabled(false);
7406  _state = newState;
7407  break;
7408 
7409  case kInitialized:
7410  _ui->actionNew_database->setEnabled(false);
7411  _ui->actionOpen_database->setEnabled(false);
7412  _ui->actionClose_database->setEnabled(true);
7413  _ui->actionEdit_database->setEnabled(false);
7414  _ui->actionStart->setEnabled(true);
7415  _ui->actionPause->setEnabled(false);
7416  _ui->actionPause->setChecked(false);
7417  _ui->actionPause->setToolTip(tr("Pause"));
7418  _ui->actionStop->setEnabled(false);
7419  _ui->actionPause_on_match->setEnabled(true);
7420  _ui->actionPause_on_local_loop_detection->setEnabled(true);
7421  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
7422  _ui->actionDump_the_memory->setEnabled(true);
7423  _ui->actionDump_the_prediction_matrix->setEnabled(true);
7424  _ui->actionDelete_memory->setEnabled(_openedDatabasePath.isEmpty());
7425  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
7426  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7427  _ui->actionGenerate_map->setEnabled(true);
7428  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
7429  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7430  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7431  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
7432 #ifdef RTABMAP_OCTOMAP
7433  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
7434 #else
7435  _ui->actionExport_octomap->setEnabled(false);
7436 #endif
7437  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7438  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7439  _ui->actionDownload_all_clouds->setEnabled(true);
7440  _ui->actionDownload_graph->setEnabled(true);
7441  _ui->menuSelect_source->setEnabled(true);
7442  _ui->actionLabel_current_location->setEnabled(true);
7443  _ui->actionSend_goal->setEnabled(true);
7444  _ui->actionCancel_goal->setEnabled(true);
7445  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
7446  _ui->actionTrigger_a_new_map->setEnabled(true);
7447  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
7448  _ui->statusbar->clearMessage();
7449  _state = newState;
7450  _oneSecondTimer->stop();
7451  break;
7452 
7453  case kStartingDetection:
7454  _ui->actionStart->setEnabled(false);
7455  _state = newState;
7456  break;
7457 
7458  case kDetecting:
7459  _ui->actionNew_database->setEnabled(false);
7460  _ui->actionOpen_database->setEnabled(false);
7461  _ui->actionClose_database->setEnabled(false);
7462  _ui->actionEdit_database->setEnabled(false);
7463  _ui->actionStart->setEnabled(false);
7464  _ui->actionPause->setEnabled(true);
7465  _ui->actionPause->setChecked(false);
7466  _ui->actionPause->setToolTip(tr("Pause"));
7467  _ui->actionStop->setEnabled(true);
7468  _ui->actionPause_on_match->setEnabled(true);
7469  _ui->actionPause_on_local_loop_detection->setEnabled(true);
7470  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
7471  _ui->actionDump_the_memory->setEnabled(false);
7472  _ui->actionDump_the_prediction_matrix->setEnabled(false);
7473  _ui->actionDelete_memory->setEnabled(false);
7474  _ui->actionPost_processing->setEnabled(false);
7475  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
7476  _ui->actionGenerate_map->setEnabled(false);
7477  _ui->menuExport_poses->setEnabled(false);
7478  _ui->actionSave_point_cloud->setEnabled(false);
7479  _ui->actionView_high_res_point_cloud->setEnabled(false);
7480  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
7481  _ui->actionExport_octomap->setEnabled(false);
7482  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
7483  _ui->actionDepth_Calibration->setEnabled(false);
7484  _ui->actionDownload_all_clouds->setEnabled(false);
7485  _ui->actionDownload_graph->setEnabled(false);
7486  _ui->menuSelect_source->setEnabled(false);
7487  _ui->actionLabel_current_location->setEnabled(true);
7488  _ui->actionSend_goal->setEnabled(true);
7489  _ui->actionCancel_goal->setEnabled(true);
7490  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
7491  _ui->actionTrigger_a_new_map->setEnabled(true);
7492  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
7493  _ui->statusbar->showMessage(tr("Detecting..."));
7494  _state = newState;
7495  _ui->label_elapsedTime->setText("00:00:00");
7496  _elapsedTime->start();
7497  _oneSecondTimer->start();
7498 
7499  _databaseUpdated = true; // if a new database is used, it won't be empty anymore...
7500 
7501  if(_camera)
7502  {
7503  _camera->start();
7504  if(_imuThread)
7505  {
7506  _imuThread->start();
7507  }
7508  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
7509  }
7510  break;
7511 
7512  case kPaused:
7513  if(_state == kPaused)
7514  {
7515  _ui->actionPause->setToolTip(tr("Pause"));
7516  _ui->actionPause->setChecked(false);
7517  _ui->statusbar->showMessage(tr("Detecting..."));
7518  _ui->actionDump_the_memory->setEnabled(false);
7519  _ui->actionDump_the_prediction_matrix->setEnabled(false);
7520  _ui->actionDelete_memory->setEnabled(false);
7521  _ui->actionPost_processing->setEnabled(false);
7522  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
7523  _ui->actionGenerate_map->setEnabled(false);
7524  _ui->menuExport_poses->setEnabled(false);
7525  _ui->actionSave_point_cloud->setEnabled(false);
7526  _ui->actionView_high_res_point_cloud->setEnabled(false);
7527  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
7528  _ui->actionExport_octomap->setEnabled(false);
7529  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
7530  _ui->actionDepth_Calibration->setEnabled(false);
7531  _ui->actionDownload_all_clouds->setEnabled(false);
7532  _ui->actionDownload_graph->setEnabled(false);
7533  _state = kDetecting;
7534  _elapsedTime->start();
7535  _oneSecondTimer->start();
7536 
7537  if(_camera)
7538  {
7539  _camera->start();
7540  if(_imuThread)
7541  {
7542  _imuThread->start();
7543  }
7544  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
7545  }
7546  }
7547  else if(_state == kDetecting)
7548  {
7549  _ui->actionPause->setToolTip(tr("Continue (shift-click for step-by-step)"));
7550  _ui->actionPause->setChecked(true);
7551  _ui->statusbar->showMessage(tr("Paused..."));
7552  _ui->actionDump_the_memory->setEnabled(true);
7553  _ui->actionDump_the_prediction_matrix->setEnabled(true);
7554  _ui->actionDelete_memory->setEnabled(false);
7555  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
7556  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7557  _ui->actionGenerate_map->setEnabled(true);
7558  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
7559  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7560  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7561  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
7562 #ifdef RTABMAP_OCTOMAP
7563  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
7564 #else
7565  _ui->actionExport_octomap->setEnabled(false);
7566 #endif
7567  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7568  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7569  _ui->actionDownload_all_clouds->setEnabled(true);
7570  _ui->actionDownload_graph->setEnabled(true);
7571  _state = kPaused;
7572  _oneSecondTimer->stop();
7573 
7574  // kill sensors
7575  if(_camera)
7576  {
7577  if(_imuThread)
7578  {
7579  _imuThread->join(true);
7580  }
7581  _camera->join(true);
7582  }
7583  }
7584  break;
7585  case kMonitoring:
7586  _ui->actionPause->setEnabled(true);
7587  _ui->actionPause->setChecked(false);
7588  _ui->actionPause->setToolTip(tr("Pause"));
7589  _ui->actionPause_on_match->setEnabled(true);
7590  _ui->actionPause_on_local_loop_detection->setEnabled(true);
7591  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
7592  _ui->actionReset_Odometry->setEnabled(true);
7593  _ui->actionPost_processing->setEnabled(false);
7594  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
7595  _ui->menuExport_poses->setEnabled(false);
7596  _ui->actionSave_point_cloud->setEnabled(false);
7597  _ui->actionView_high_res_point_cloud->setEnabled(false);
7598  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
7599  _ui->actionExport_octomap->setEnabled(false);
7600  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
7601  _ui->actionDepth_Calibration->setEnabled(false);
7602  _ui->actionDelete_memory->setEnabled(true);
7603  _ui->actionDownload_all_clouds->setEnabled(true);
7604  _ui->actionDownload_graph->setEnabled(true);
7605  _ui->actionTrigger_a_new_map->setEnabled(true);
7606  _ui->actionLabel_current_location->setEnabled(true);
7607  _ui->actionSend_goal->setEnabled(true);
7608  _ui->actionCancel_goal->setEnabled(true);
7609  _ui->statusbar->showMessage(tr("Monitoring..."));
7610  _state = newState;
7611  _elapsedTime->start();
7612  _oneSecondTimer->start();
7613  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResume));
7614  break;
7615  case kMonitoringPaused:
7616  _ui->actionPause->setToolTip(tr("Continue"));
7617  _ui->actionPause->setChecked(true);
7618  _ui->actionPause->setEnabled(true);
7619  _ui->actionPause_on_match->setEnabled(true);
7620  _ui->actionPause_on_local_loop_detection->setEnabled(true);
7621  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
7622  _ui->actionReset_Odometry->setEnabled(true);
7623  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
7624  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7625  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
7626  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7627  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
7628  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
7629 #ifdef RTABMAP_OCTOMAP
7630  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
7631 #else
7632  _ui->actionExport_octomap->setEnabled(false);
7633 #endif
7634  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7635  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
7636  _ui->actionDelete_memory->setEnabled(true);
7637  _ui->actionDownload_all_clouds->setEnabled(true);
7638  _ui->actionDownload_graph->setEnabled(true);
7639  _ui->actionTrigger_a_new_map->setEnabled(true);
7640  _ui->actionLabel_current_location->setEnabled(true);
7641  _ui->actionSend_goal->setEnabled(true);
7642  _ui->actionCancel_goal->setEnabled(true);
7643  _ui->statusbar->showMessage(tr("Monitoring paused..."));
7644  _state = newState;
7645  _oneSecondTimer->stop();
7646  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause));
7647  break;
7648  default:
7649  break;
7650  }
7651 
7652 }
7653 
7654 }
rtabmap::RtabmapEvent3DMap::getPoses
const std::map< int, Transform > & getPoses() const
Definition: RtabmapEvent.h:186
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
ULogger::setPrintThreadId
static void setPrintThreadId(bool printThreadId)
Definition: ULogger.h:309
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:118
rtabmap::Statistics::extended
bool extended() const
Definition: Statistics.h:201
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::RtabmapEventInit::getInfo
const std::string & getInfo() const
Definition: RtabmapEvent.h:159
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::PreferencesDialog::getCloudColorScheme
int getCloudColorScheme(int index) const
Definition: PreferencesDialog.cpp:4628
rtabmap::MainWindow::_cachedEmptyClouds
std::set< int > _cachedEmptyClouds
Definition: MainWindow.h:341
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
rtabmap::OdometryEvent::data
SensorData & data()
Definition: OdometryEvent.h:69
rtabmap::MainWindow::openPreferences
virtual void openPreferences()
Definition: MainWindow.cpp:6423
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::ProgressDialog::setAutoClose
void setAutoClose(bool on, int delayedClosingTimeMsec=-1)
Definition: ProgressDialog.cpp:88
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
rtabmap::util3d::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
Definition: util3d_filtering.cpp:860
rtabmap::MainWindow::_depthCalibrationDialog
DepthCalibrationDialog * _depthCalibrationDialog
Definition: MainWindow.h:311
rtabmap::MainWindow::processStats
virtual void processStats(const rtabmap::Statistics &stat)
Definition: MainWindow.cpp:1534
rtabmap::MainWindow::setAspectRatio
void setAspectRatio(int w, int h)
Definition: MainWindow.cpp:6546
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:120
rtabmap::PreferencesDialog::createCamera
Camera * createCamera(bool useRawImages=false, bool useColor=true)
Definition: PreferencesDialog.cpp:4948
UTimer::restart
double restart()
Definition: UTimer.h:94
rtabmap::PreferencesDialog::getScanMinRange
double getScanMinRange(int index) const
Definition: PreferencesDialog.cpp:4659
rtabmap::PreferencesDialog::saveMainWindowState
void saveMainWindowState(const QMainWindow *mainWindow)
Definition: PreferencesDialog.cpp:2968
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:63
rtabmap::PreferencesDialog::updateParameters
void updateParameters(const ParametersMap &parameters, bool setOtherParametersToDefault=false)
Definition: PreferencesDialog.cpp:3237
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:69
rtabmap::MainWindow::_waypoints
QStringList _waypoints
Definition: MainWindow.h:328
UTimer
Definition: UTimer.h:46
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), bool g2oRobust=false)
Definition: Graph.cpp:53
UEventsManager::createPipe
static void createPipe(const UEventsSender *sender, const UEventsHandler *receiver, const std::string &eventName)
Definition: UEventsManager.cpp:67
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:66
rtabmap::MainWindow::applyPrefSettings
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
Definition: MainWindow.cpp:3994
rtabmap::OdometryInfo::localBundleModels
std::map< int, CameraModel > localBundleModels
Definition: OdometryInfo.h:100
UThread::start
void start()
Definition: UThread.cpp:122
rtabmap::CameraThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: CameraThread.h:63
rtabmap::OdometryInfo::transformGroundTruth
Transform transformGroundTruth
Definition: OdometryInfo.h:108
rtabmap::OccupancyGrid::getCellSize
float getCellSize() const
Definition: OccupancyGrid.h:58
UFile::length
long length()
Definition: UFile.h:110
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1045
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::PreferencesDialog::getSubtractFilteringRadius
double getSubtractFilteringRadius() const
Definition: PreferencesDialog.cpp:4721
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
UEvent
Definition: UEvent.h:57
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::IMUThread::init
bool init(const std::string &path)
Definition: IMUThread.cpp:50
rtabmap::PreferencesDialog::getCeilingFilteringHeight
double getCeilingFilteringHeight() const
Definition: PreferencesDialog.cpp:4534
rtabmap::MainWindow::_newDatabasePathOutput
QString _newDatabasePathOutput
Definition: MainWindow.h:322
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:107
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:132
rtabmap::CloudViewer::updateCameraFrustum
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
Definition: CloudViewer.cpp:2468
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:89
rtabmap::PreferencesDialog::getAllParameters
rtabmap::ParametersMap getAllParameters() const
Definition: PreferencesDialog.cpp:3211
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
Definition: util3d_filtering.cpp:507
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Signature::setWords3
void setWords3(const std::multimap< int, cv::Point3f > &words3)
Definition: Signature.h:115
octomap::OcTreeBaseImpl::memoryUsage
virtual size_t memoryUsage() const
rtabmap::MainWindow::showEvent
virtual void showEvent(QShowEvent *anEvent)
Definition: MainWindow.cpp:4265
rtabmap::CloudViewer::setCloudVisibility
void setCloudVisibility(const std::string &id, bool isVisible)
Definition: CloudViewer.cpp:2539
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::LoopClosureViewer::setMaxDepth
void setMaxDepth(int maxDepth)
Definition: LoopClosureViewer.h:58
rtabmap::CloudViewer::addOrUpdateText
void addOrUpdateText(const std::string &id, const std::string &text, const Transform &position, double scale, const QColor &color, bool foreground=true)
Definition: CloudViewer.cpp:1886
rtabmap::OdometryInfo::transformFiltered
Transform transformFiltered
Definition: OdometryInfo.h:107
rtabmap::Statistics::getSignatures
const std::map< int, Signature > & getSignatures() const
Definition: Statistics.h:207
rtabmap::OctoMap::update
void update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:367
rtabmap::MainWindow::_cachedLocalizationsCount
std::map< int, float > _cachedLocalizationsCount
Definition: MainWindow.h:344
rtabmap::Signature::setWords
void setWords(const std::multimap< int, cv::KeyPoint > &words)
Definition: Signature.cpp:231
rtabmap::PreferencesDialog::setMonitoringState
void setMonitoringState(bool monitoringState)
Definition: PreferencesDialog.h:282
rtabmap::Registration::computeTransformation
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:166
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::RtabmapEvent::getStats
const Statistics & getStats() const
Definition: RtabmapEvent.h:50
rtabmap::Statistics::likelihood
const std::map< int, float > & likelihood() const
Definition: Statistics.h:216
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
Definition: DBDriver.cpp:813
rtabmap::MainWindow::_preferencesDialog
PreferencesDialog * _preferencesDialog
Definition: MainWindow.h:306
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:116
rtabmap::CloudViewer::removeFrustum
void removeFrustum(const std::string &id)
Definition: CloudViewer.cpp:1797
rtabmap::Statistics::proximityDetectionId
int proximityDetectionId() const
Definition: Statistics.h:204
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2399
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:155
rtabmap::Statistics::posterior
const std::map< int, float > & posterior() const
Definition: Statistics.h:215
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2082
rtabmap::util3d::filterNotUsedVerticesFromMesh
std::vector< int > RTABMAP_EXP filterNotUsedVerticesFromMesh(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGBNormal > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
Definition: util3d_surface.cpp:320
rtabmap::PreferencesDialog::getIniFilePath
virtual QString getIniFilePath() const
Definition: PreferencesDialog.cpp:1828
rtabmap::MainWindow::_camera
rtabmap::CameraThread * _camera
Definition: MainWindow.h:301
rtabmap::CameraThread::enableBilateralFiltering
void enableBilateralFiltering(float sigmaS, float sigmaR)
Definition: CameraThread.cpp:109
rtabmap::OdometryInfo::copyWithoutData
OdometryInfo copyWithoutData() const
Definition: OdometryInfo.h:62
UThread::kill
void kill()
Definition: UThread.cpp:48
rtabmap::MainWindow::_autoScreenCaptureOdomSync
bool _autoScreenCaptureOdomSync
Definition: MainWindow.h:373
UThread::isRunning
bool isRunning() const
Definition: UThread.cpp:245
rtabmap::MainWindow::rtabmapLabelErrorReceived
void rtabmapLabelErrorReceived(int id, const QString &label)
rtabmap::MainWindow::stopDetection
virtual void stopDetection()
Definition: MainWindow.cpp:5041
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::MainWindow::timeLimitChanged
void timeLimitChanged(float)
rtabmap::RtabmapEvent3DMap::getConstraints
const std::multimap< int, Link > & getConstraints() const
Definition: RtabmapEvent.h:187
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
rtabmap::PreferencesDialog::getCloudRoiRatios
std::vector< float > getCloudRoiRatios(int index) const
Definition: PreferencesDialog.cpp:4610
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1203
rtabmap::graph::findNearestNodes
std::vector< int > RTABMAP_EXP findNearestNodes(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)
Definition: Graph.cpp:1883
rtabmap::MainWindow::_openedDatabasePath
QString _openedDatabasePath
Definition: MainWindow.h:323
rtabmap::MainWindow::_currentLinksMap
std::multimap< int, Link > _currentLinksMap
Definition: MainWindow.h:336
rtabmap::ExportCloudsDialog::viewClouds
void viewClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters)
Definition: ExportCloudsDialog.cpp:929
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:110
rtabmap::MainWindow::_odomThread
rtabmap::OdometryThread * _odomThread
Definition: MainWindow.h:302
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::MainWindow::openDatabase
virtual void openDatabase()
Definition: MainWindow.cpp:4551
rtabmap::RtabmapEventInit::getStatus
Status getStatus() const
Definition: RtabmapEvent.h:158
rtabmap::MainWindow::imgRateChanged
void imgRateChanged(double)
rtabmap::MainWindow::changeState
virtual void changeState(MainWindow::State state)
Definition: MainWindow.cpp:7271
rtabmap::LoopClosureViewer::setData
void setData(const Signature &sA, const Signature &sB)
Definition: LoopClosureViewer.cpp:60
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:194
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
rtabmap::SensorData::setCameraModels
void setCameraModels(const std::vector< CameraModel > &models)
Definition: SensorData.h:168
rtabmap::CloudViewer::getDefaultBackgroundColor
const QColor & getDefaultBackgroundColor() const
Definition: CloudViewer.cpp:2514
rtabmap::PdfPlotCurve::setData
void setData(const QMap< int, float > &dataMap, const QMap< int, int > &weightsMap)
Definition: PdfPlot.cpp:136
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::ExportBundlerDialog::setWorkingDirectory
void setWorkingDirectory(const QString &path)
Definition: ExportBundlerDialog.cpp:89
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:70
rtabmap::MainWindow::loopClosureViewer
rtabmap::LoopClosureViewer * loopClosureViewer() const
Definition: MainWindow.h:289
rtabmap::MainWindow::selectStereoFlyCapture2
void selectStereoFlyCapture2()
Definition: MainWindow.cpp:6043
rtabmap::OccupancyGrid::update
void update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:685
UConversion.h
Some conversion functions.
rtabmap::MainWindow::openPreferencesSource
virtual void openPreferencesSource()
Definition: MainWindow.cpp:6429
rtabmap::MainWindow::setupMainLayout
void setupMainLayout(bool vertical)
Definition: MainWindow.cpp:660
rtabmap::MainWindow::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: MainWindow.cpp:4292
rtabmap::MainWindow::updateNodeVisibility
void updateNodeVisibility(int, bool)
Definition: MainWindow.cpp:3597
rtabmap::PreferencesDialog::getScanMaxRange
double getScanMaxRange(int index) const
Definition: PreferencesDialog.cpp:4654
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::CameraThread::setImageRate
void setImageRate(float imageRate)
Definition: CameraThread.cpp:81
ULogger::setTreadIdFilter
static void setTreadIdFilter(const std::set< unsigned long > &ids)
Definition: ULogger.h:354
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::PreferencesDialog::getGeneralLoggerThreads
std::vector< std::string > getGeneralLoggerThreads() const
Definition: PreferencesDialog.cpp:4410
rtabmap::MainWindow::_rectCameraModels
std::vector< CameraModel > _rectCameraModels
Definition: MainWindow.h:330
rtabmap::SensorData::groundTruth
const Transform & groundTruth() const
Definition: SensorData.h:232
rtabmap::MainWindow::_newDatabasePath
QString _newDatabasePath
Definition: MainWindow.h:321
rtabmap::CameraOpenNI2::available
static bool available()
Definition: CameraRGBD.cpp:382
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
rtabmap::MainWindow::_exportPosesFileName
QMap< int, QString > _exportPosesFileName
Definition: MainWindow.h:372
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::MainWindow::saveConfigGUI
virtual void saveConfigGUI()
Definition: MainWindow.cpp:4447
rtabmap::MainWindow::_createdCloudsMemoryUsage
long _createdCloudsMemoryUsage
Definition: MainWindow.h:340
rtabmap::RtabmapGlobalPathEvent::getGoalLabel
const std::string & getGoalLabel() const
Definition: RtabmapEvent.h:222
rtabmap::MainWindow::stateChanged
void stateChanged(MainWindow::State)
rtabmap::Statistics::weights
const std::map< int, int > & weights() const
Definition: Statistics.h:214
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:46
rtabmap::MainWindow::pauseDetection
virtual void pauseDetection()
Definition: MainWindow.cpp:5007
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:2471
UFATAL
#define UFATAL(...)
rtabmap::MainWindow::createAndAddCloudToMap
std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > createAndAddCloudToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:2758
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_EXP extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:1646
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::PreferencesDialog::getCloudVoxelSizeScan
double getCloudVoxelSizeScan(int index) const
Definition: PreferencesDialog.cpp:4664
rtabmap::PreferencesDialog::getIMULocalTransform
Transform getIMULocalTransform() const
Definition: PreferencesDialog.cpp:4869
rtabmap::SensorData::setUserDataRaw
void setUserDataRaw(const cv::Mat &userDataRaw)
Definition: SensorData.cpp:436
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28
util3d_registration.h
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:173
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:104
rtabmap::PreferencesDialog::getCloudOpacity
double getCloudOpacity(int index) const
Definition: PreferencesDialog.cpp:4633
rtabmap::MainWindow::resetOdometry
virtual void resetOdometry()
Definition: MainWindow.cpp:7197
rtabmap::MainWindow::state
const State & state() const
Definition: MainWindow.h:272
rtabmap::MainWindow::rtabmapGlobalPathEventReceived
void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &event)
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::LoopClosureViewer::updateView
void updateView(const Transform &AtoB=Transform(), const ParametersMap &parameters=ParametersMap())
Definition: LoopClosureViewer.cpp:70
rtabmap::SensorData::setImageRaw
void setImageRaw(const cv::Mat &imageRaw)
Definition: SensorData.h:164
rtabmap::PreferencesDialog::loadMainWindowState
void loadMainWindowState(QMainWindow *mainWindow, bool &maximized, bool &statusBarShown)
Definition: PreferencesDialog.cpp:2994
rtabmap::CloudViewer::addCloudTextureMesh
bool addCloudTextureMesh(const std::string &id, const pcl::TextureMesh::Ptr &textureMesh, const cv::Mat &texture, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:767
rtabmap::CameraOpenNICV::available
static bool available()
Definition: CameraRGBD.cpp:272
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor())
Definition: CloudViewer.cpp:455
rtabmap::MainWindow::_currentPosesMap
std::map< int, Transform > _currentPosesMap
Definition: MainWindow.h:334
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:105
rtabmap::MainWindow::_odometryCorrection
Transform _odometryCorrection
Definition: MainWindow.h:353
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
util3d_mapping.h
rtabmap::MainWindow::_previousCloud
std::pair< int, std::pair< std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr >, pcl::IndicesPtr > > _previousCloud
Definition: MainWindow.h:342
rtabmap::PreferencesDialog::getSourceDriver
PreferencesDialog::Src getSourceDriver() const
Definition: PreferencesDialog.cpp:4797
rtabmap::PreferencesDialog::saveCustomConfig
void saveCustomConfig(const QString &section, const QString &key, const QString &value)
Definition: PreferencesDialog.cpp:3174
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
UASSERT
#define UASSERT(condition)
rtabmap::PreferencesDialog::getSourceDistortionModel
QString getSourceDistortionModel() const
Definition: PreferencesDialog.cpp:4895
rtabmap::CameraThread::setColorOnly
void setColorOnly(bool colorOnly)
Definition: CameraThread.h:64
rtabmap::MainWindow::statsReceived
void statsReceived(const rtabmap::Statistics &)
rtabmap::SensorData::clearOccupancyGridRaw
void clearOccupancyGridRaw()
Definition: SensorData.h:216
rtabmap::MainWindow::_createdScans
std::map< int, LaserScan > _createdScans
Definition: MainWindow.h:346
rtabmap::MainWindow::processCameraInfo
void processCameraInfo(const rtabmap::CameraInfo &info)
Definition: MainWindow.cpp:926
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:223
rtabmap::PreferencesDialog::getSourceScanNormalsRadius
double getSourceScanNormalsRadius() const
Definition: PreferencesDialog.cpp:4943
rtabmap::MainWindow::resizeEvent
virtual void resizeEvent(QResizeEvent *anEvent)
Definition: MainWindow.cpp:4284
rtabmap::ProgressDialog::isCanceled
bool isCanceled() const
Definition: ProgressDialog.h:57
rtabmap::MainWindow::setLoopClosureViewer
void setLoopClosureViewer(rtabmap::LoopClosureViewer *loopClosureViewer)
Definition: MainWindow.cpp:686
rtabmap::PostProcessingDialog::sbaType
Optimizer::Type sbaType() const
Definition: PostProcessingDialog.cpp:199
rtabmap::OctoMap::addToCache
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:343
rtabmap::CameraInfo::timeStereoExposureCompensation
float timeStereoExposureCompensation
Definition: CameraInfo.h:62
rtabmap::OdometryEvent::pose
const Transform & pose() const
Definition: OdometryEvent.h:71
rtabmap::OctoMap::addedNodes
const std::map< int, Transform > & addedNodes() const
Definition: OctoMap.h:177
rtabmap::Camera::isCalibrated
virtual bool isCalibrated() const =0
rtabmap::MainWindow::handleEvent
virtual bool handleEvent(UEvent *anEvent)
Definition: MainWindow.cpp:790
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::MainWindow::mappingModeChanged
void mappingModeChanged(bool)
rtabmap::MainWindow::changeDetectionRateSetting
void changeDetectionRateSetting()
Definition: MainWindow.cpp:4352
rtabmap::DepthCalibrationDialog::calibrate
void calibrate(const std::map< int, Transform > &poses, const QMap< int, Signature > &cachedSignatures, const QString &workingDirectory, const ParametersMap &parameters)
Definition: DepthCalibrationDialog.cpp:210
true
#define true
Definition: ConvertUTF.c:57
rtabmap::MainWindow::rtabmapEventInitReceived
void rtabmapEventInitReceived(int status, const QString &info)
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
ULogger::setPrintTime
static void setPrintTime(bool printTime)
Definition: ULogger.h:273
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::MainWindow::_refIds
QVector< int > _refIds
Definition: MainWindow.h:378
rtabmap::MainWindow::_lastIds
QSet< int > _lastIds
Definition: MainWindow.h:314
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::LoopClosureViewer::setDecimation
void setDecimation(int decimation)
Definition: LoopClosureViewer.h:57
rtabmap::OdometryInfo::cornerInliers
std::vector< int > cornerInliers
Definition: OdometryInfo.h:122
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack)
Definition: UCv2Qt.h:44
rtabmap::CloudViewer::addOrUpdateFrustum
void addOrUpdateFrustum(const std::string &id, const Transform &transform, const Transform &localTransform, double scale, const QColor &color=QColor())
Definition: CloudViewer.cpp:1677
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::PreferencesDialog::getScanNormalRadiusSearch
double getScanNormalRadiusSearch() const
Definition: PreferencesDialog.cpp:4562
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
Definition: OccupancyGrid.cpp:675
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2416
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:528
rtabmap::MainWindow::cameraInfoReceived
void cameraInfoReceived(const rtabmap::CameraInfo &)
util3d_filtering.h
rtabmap::MainWindow::_octomap
rtabmap::OctoMap * _octomap
Definition: MainWindow.h:349
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:1974
rtabmap::PreferencesDialog::getDownsamplingStepScan
int getDownsamplingStepScan(int index) const
Definition: PreferencesDialog.cpp:4649
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:62
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:108
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:331
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:109
rtabmap::MainWindow::_likelihoodCurve
PdfPlotCurve * _likelihoodCurve
Definition: MainWindow.h:362
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:684
rtabmap::MainWindow::_imuThread
rtabmap::IMUThread * _imuThread
Definition: MainWindow.h:303
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1031
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::PreferencesDialog::getCloudDecimation
int getCloudDecimation(int index) const
Definition: PreferencesDialog.cpp:4595
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:67
rtabmap::OdometryInfo::localBundlePoses
std::map< int, Transform > localBundlePoses
Definition: OdometryInfo.h:99
rtabmap::MainWindow::_cachedClouds
std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > _cachedClouds
Definition: MainWindow.h:339
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:2592
ULogger::setEventLevel
static void setEventLevel(ULogger::Level eventSentLevel)
Definition: ULogger.h:348
rtabmap::PreferencesDialog::getScanPointSize
int getScanPointSize(int index) const
Definition: PreferencesDialog.cpp:4679
opengl_world_T_rtabmap_world
static const rtabmap::Transform opengl_world_T_rtabmap_world(0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f)
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
Definition: Signature.h:112
rtabmap::PreferencesDialog::isSourceStereoExposureCompensation
bool isSourceStereoExposureCompensation() const
Definition: PreferencesDialog.cpp:4919
rtabmap::OctoMap::createCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true) const
Definition: OctoMap.cpp:920
rtabmap::MainWindow::anchorCloudsToGroundTruth
void anchorCloudsToGroundTruth()
Definition: MainWindow.cpp:6274
rtabmap::MainWindow::_currentLabels
std::map< int, std::string > _currentLabels
Definition: MainWindow.h:338
rtabmap::MainWindow::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: MainWindow.cpp:696
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
Definition: OctoMap.h:189
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters)
Definition: Odometry.cpp:53
rtabmap::MainWindow::progressDialog
rtabmap::ProgressDialog * progressDialog()
Definition: MainWindow.h:287
rtabmap::MainWindow::exportPoses
void exportPoses(int format)
Definition: MainWindow.cpp:5182
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2347
rtabmap::MainWindow::updateParameters
void updateParameters(const rtabmap::ParametersMap &parameters)
Definition: MainWindow.cpp:4424
rtabmap::PreferencesDialog::saveWindowGeometry
void saveWindowGeometry(const QWidget *window)
Definition: PreferencesDialog.cpp:2923
rtabmap::SensorData::getMemoryUsed
long getMemoryUsed() const
Definition: SensorData.cpp:837
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
rtabmap::OccupancyGrid::cacheSize
int cacheSize() const
Definition: OccupancyGrid.h:66
rtabmap::PreferencesDialog::isFeaturesShown
bool isFeaturesShown(int index) const
Definition: PreferencesDialog.cpp:4685
rtabmap::MainWindow::openHelp
virtual void openHelp()
Definition: MainWindow.cpp:6350
rtabmap::DBDriver::loadNodeData
void loadNodeData(std::list< Signature * > &signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:622
rtabmap::MainWindow::processRtabmapLabelErrorEvent
void processRtabmapLabelErrorEvent(int id, const QString &label)
Definition: MainWindow.cpp:3970
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:672
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:162
UTimer::start
void start()
Definition: UTimer.cpp:80
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
rtabmap::OdometryEvent::info
const OdometryInfo & info() const
Definition: OdometryEvent.h:89
clams::calibrate
DiscreteDepthDistortionModel RTABMAP_EXP calibrate(const std::map< int, rtabmap::SensorData > &sequence, const std::map< int, rtabmap::Transform > &trajectory, const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, double coneRadius=0.02, double coneStdevThresh=0.03)
Definition: slam_calibrator.cpp:40
rtabmap::CameraModel::P
cv::Mat P() const
Definition: CameraModel.h:106
rtabmap::CloudViewer::getBackgroundColor
const QColor & getBackgroundColor() const
Definition: CloudViewer.cpp:2528
rtabmap::PreferencesDialog::getCloudPointSize
int getCloudPointSize(int index) const
Definition: PreferencesDialog.cpp:4638
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::CameraThread::setDistortionModel
void setDistortionModel(const std::string &path)
Definition: CameraThread.cpp:89
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:64
rtabmap::Statistics::loopClosureId
int loopClosureId() const
Definition: Statistics.h:203
util3d_transforms.h
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:153
rtabmap::MainWindow::_cloudViewer
CloudViewer * _cloudViewer
Definition: MainWindow.h:367
rtabmap::DatabaseViewer::openDatabase
bool openDatabase(const QString &path)
Definition: DatabaseViewer.cpp:705
rtabmap::CloudViewer::getAddedClouds
const QMap< std::string, Transform > & getAddedClouds() const
Definition: CloudViewer.h:307
rtabmap::MainWindow::deleteMemory
virtual void deleteMemory()
Definition: MainWindow.cpp:5911
rtabmap::MainWindow::_posteriorCurve
PdfPlotCurve * _posteriorCurve
Definition: MainWindow.h:361
rtabmap::PreferencesDialog::isScansShown
bool isScansShown(int index) const
Definition: PreferencesDialog.cpp:4644
rtabmap::CameraThread::setMirroringEnabled
void setMirroringEnabled(bool enabled)
Definition: CameraThread.h:62
rtabmap::CloudViewer::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: CloudViewer.cpp:2533
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
LOG_FILE_NAME
#define LOG_FILE_NAME
Definition: MainWindow.cpp:122
rtabmap::CloudViewer::updateFrustumPose
bool updateFrustumPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:1754
rtabmap::ExportCloudsDialog::exportClouds
void exportClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters)
Definition: ExportCloudsDialog.cpp:867
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1157
rtabmap::PreferencesDialog::isFrustumsShown
bool isFrustumsShown(int index) const
Definition: PreferencesDialog.cpp:4690
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
rtabmap::Statistics::loopClosureTransform
const Transform & loopClosureTransform() const
Definition: Statistics.h:212
rtabmap::MainWindow::triggerNewMap
virtual void triggerNewMap()
Definition: MainWindow.cpp:7203
rtabmap::PreferencesDialog::getScanCeilingFilteringHeight
double getScanCeilingFilteringHeight() const
Definition: PreferencesDialog.cpp:4550
rtabmap::MainWindow::rtabmapEvent3DMapProcessed
void rtabmapEvent3DMapProcessed()
rtabmap::MainWindow::_currentGTPosesMap
std::map< int, Transform > _currentGTPosesMap
Definition: MainWindow.h:335
rtabmap::MainWindow::setMonitoringState
void setMonitoringState(bool pauseChecked=false)
Definition: MainWindow.cpp:7265
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:2618
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:116
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::MainWindow::_createdFeatures
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > _createdFeatures
Definition: MainWindow.h:351
rtabmap::CameraThread::setImageDecimation
void setImageDecimation(int decimation)
Definition: CameraThread.h:65
rtabmap::MainWindow::_cachedSignatures
QMap< int, Signature > _cachedSignatures
Definition: MainWindow.h:332
UEventsHandler::registerToEventsManager
void registerToEventsManager()
Definition: UEventsHandler.cpp:29
initGuiResource
static void initGuiResource()
Definition: MainWindow.cpp:129
rtabmap::CloudViewer::getCloudVisibility
bool getCloudVisibility(const std::string &id)
Definition: CloudViewer.cpp:2559
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:416
rtabmap::MainWindow::clearTheCache
virtual void clearTheCache()
Definition: MainWindow.cpp:6284
rtabmap::MainWindow::_autoScreenCaptureCachedImages
QMap< QString, QByteArray > _autoScreenCaptureCachedImages
Definition: MainWindow.h:376
rtabmap::MainWindow::MainWindow
MainWindow(PreferencesDialog *prefDialog=0, QWidget *parent=0, bool showSplashScreen=true)
Definition: MainWindow.cpp:134
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Statistics::poses
const std::map< int, Transform > & poses() const
Definition: Statistics.h:209
rtabmap::PreferencesDialog::getScanOpacity
double getScanOpacity(int index) const
Definition: PreferencesDialog.cpp:4674
rtabmap::MainWindow::setCloudViewer
void setCloudViewer(rtabmap::CloudViewer *cloudViewer)
Definition: MainWindow.cpp:672
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:775
rtabmap::MainWindow::moveEvent
virtual void moveEvent(QMoveEvent *anEvent)
Definition: MainWindow.cpp:4271
rtabmap::MainWindow::processRtabmapEvent3DMap
void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &event)
Definition: MainWindow.cpp:3798
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:897
rtabmap::MainWindow::_rawLikelihoodCurve
PdfPlotCurve * _rawLikelihoodCurve
Definition: MainWindow.h:363
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:69
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:695
rtabmap::MainWindow::updateMapCloud
void updateMapCloud(const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, const std::map< int, int > &mapIds, const std::map< int, std::string > &labels, const std::map< int, Transform > &groundTruths, bool verboseProgress=false, std::map< std::string, float > *stats=0)
Definition: MainWindow.cpp:2048
rtabmap::RtabmapGlobalPathEvent::getPoses
const std::vector< std::pair< int, Transform > > & getPoses() const
Definition: RtabmapEvent.h:224
rtabmap::PreferencesDialog::loadCustomConfig
QString loadCustomConfig(const QString &section, const QString &key)
Definition: PreferencesDialog.cpp:3191
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::PreferencesDialog::getParameter
std::string getParameter(const std::string &key) const
Definition: PreferencesDialog.cpp:3226
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:1825
rtabmap::Camera::odomProvided
virtual bool odomProvided() const
Definition: Camera.h:59
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::MainWindow::_occupancyGrid
rtabmap::OccupancyGrid * _occupancyGrid
Definition: MainWindow.h:348
rtabmap::MainWindow::setDefaultViews
virtual void setDefaultViews()
Definition: MainWindow.cpp:6436
rtabmap::CameraThread::setScanFromDepth
void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f)
Definition: CameraThread.h:72
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:38
rtabmap::CameraThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: CameraThread.h:66
rtabmap::MainWindow::getWorkingDirectory
QString getWorkingDirectory() const
Definition: MainWindow.cpp:5944
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:362
rtabmap::MainWindow::startDetection
virtual void startDetection()
Definition: MainWindow.cpp:4744
rtabmap::MainWindow::_cachedWordsCount
std::map< int, float > _cachedWordsCount
Definition: MainWindow.h:343
rtabmap::MainWindow::closeDatabase
virtual bool closeDatabase()
Definition: MainWindow.cpp:4655
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:115
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::PreferencesDialog::saveWidgetState
void saveWidgetState(const QWidget *widget)
Definition: PreferencesDialog.cpp:3026
rtabmap::MainWindow::captureScreen
QString captureScreen(bool cacheInRAM=false, bool png=true)
Definition: MainWindow.cpp:4367
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
rtabmap::MainWindow::detectionRateChanged
void detectionRateChanged(double)
rtabmap::CameraEvent::info
const CameraInfo & info() const
Definition: CameraEvent.h:81
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:2488
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:120
rtabmap::MainWindow::_currentMapIds
std::map< int, int > _currentMapIds
Definition: MainWindow.h:337
uKeysList
std::list< K > uKeysList(const std::multimap< K, V > &mm)
Definition: UStl.h:84
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:825
rtabmap::MainWindow::postGoal
void postGoal(const QString &goal)
Definition: MainWindow.cpp:6102
rtabmap::PreferencesDialog::getFeaturesPointSize
int getFeaturesPointSize(int index) const
Definition: PreferencesDialog.cpp:4695
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:448
rtabmap::MainWindow::processRtabmapEventInit
void processRtabmapEventInit(int status, const QString &info)
Definition: MainWindow.cpp:3672
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::OctoMap::writeBinary
bool writeBinary(const std::string &path)
Definition: OctoMap.cpp:1098
UEventsManager::addHandler
static void addHandler(UEventsHandler *handler)
Definition: UEventsManager.cpp:28
rtabmap::MainWindow::loopClosureThrChanged
void loopClosureThrChanged(float)
rtabmap::databaseRecovery
bool RTABMAP_EXP databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:65
rtabmap::Statistics::constraints
const std::multimap< int, Link > & constraints() const
Definition: Statistics.h:210
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
rtabmap::DatabaseViewer::isSavedMaximized
bool isSavedMaximized() const
Definition: DatabaseViewer.h:72
rtabmap::CloudViewer::getAddedFrustums
const QMap< std::string, Transform > & getAddedFrustums() const
Definition: CloudViewer.h:268
UERROR
#define UERROR(...)
rtabmap::Statistics::defaultData
static const std::map< std::string, float > & defaultData()
Definition: Statistics.cpp:36
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::MainWindow::downloadPoseGraph
virtual void downloadPoseGraph()
Definition: MainWindow.cpp:6229
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
rtabmap::PreferencesDialog::loadWindowGeometry
void loadWindowGeometry(QWidget *window)
Definition: PreferencesDialog.cpp:2943
rtabmap::CloudViewer::setBackfaceCulling
void setBackfaceCulling(bool enabled, bool frontfaceCulling)
Definition: CloudViewer.cpp:2220
rtabmap::MainWindow::_progressDialog
ProgressDialog * _progressDialog
Definition: MainWindow.h:365
rtabmap::MainWindow::_postProcessingDialog
PostProcessingDialog * _postProcessingDialog
Definition: MainWindow.h:310
rtabmap::Statistics::currentGoalId
int currentGoalId() const
Definition: Statistics.h:219
rtabmap::CameraInfo::timeImageDecimation
float timeImageDecimation
Definition: CameraInfo.h:63
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::MainWindow::_aboutDialog
AboutDialog * _aboutDialog
Definition: MainWindow.h:307
rtabmap::CloudViewer::setCameraTargetFollow
void setCameraTargetFollow(bool enabled=true)
Definition: CloudViewer.cpp:2607
rtabmap::MainWindow::_exportBundlerDialog
ExportBundlerDialog * _exportBundlerDialog
Definition: MainWindow.h:309
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:72
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:121
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:2574
rtabmap::OccupancyGrid::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: OccupancyGrid.cpp:92
rtabmap::MainWindow::alignPosesToGroundTruth
Transform alignPosesToGroundTruth(const std::map< int, Transform > &poses, const std::map< int, Transform > &groundTruth)
Definition: MainWindow.cpp:3543
rtabmap::PreferencesDialog::getSourceScanFromDepthMaxDepth
double getSourceScanFromDepthMaxDepth() const
Definition: PreferencesDialog.cpp:4931
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:91
rtabmap::PreferencesDialog::getScanFloorFilteringHeight
double getScanFloorFilteringHeight() const
Definition: PreferencesDialog.cpp:4554
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:887
rtabmap::CameraEvent::data
const SensorData & data() const
Definition: CameraEvent.h:79
rtabmap::RtabmapLabelErrorEvent::label
const std::string & label() const
Definition: RtabmapEvent.h:242
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::MainWindow::_ui
Ui_mainWindow * _ui
Definition: MainWindow.h:298
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, bool forceGroundNormalsUp=false)
Definition: util3d_filtering.cpp:74
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1043
rtabmap::Optimizer::getConnectedGraph
static void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut, int depth=0)
Definition: Optimizer.cpp:157
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:805
rtabmap::CameraInfo::timeBilateralFiltering
float timeBilateralFiltering
Definition: CameraInfo.h:66
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:103
rtabmap::CloudViewer::addCloudMesh
bool addCloudMesh(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< pcl::Vertices > &polygons, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:632
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2381
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:344
rtabmap::MainWindow::_graphSavingFileName
QString _graphSavingFileName
Definition: MainWindow.h:370
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::PdfPlotCurve::clear
virtual void clear()
Definition: PdfPlot.cpp:131
rtabmap::MainWindow::eventFilter
virtual bool eventFilter(QObject *obj, QEvent *event)
Definition: MainWindow.cpp:4301
rtabmap::MainWindow::_oneSecondTimer
QTimer * _oneSecondTimer
Definition: MainWindow.h:357
rtabmap::PreferencesDialog::getSubtractFilteringAngle
double getSubtractFilteringAngle() const
Definition: PreferencesDialog.cpp:4725
rtabmap::MainWindow::processOdometry
void processOdometry(const rtabmap::OdometryEvent &odom, bool dataIgnored)
Definition: MainWindow.cpp:945
rtabmap::MainWindow::rtabmapEvent3DMapReceived
void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &event)
rtabmap::PreferencesDialog::getCloudMaxDepth
double getCloudMaxDepth(int index) const
Definition: PreferencesDialog.cpp:4600
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraRGBD.cpp:2065
rtabmap::MainWindow::createAndAddScanToMap
void createAndAddScanToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:3133
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:407
rtabmap::PreferencesDialog::loadWidgetState
void loadWidgetState(QWidget *widget)
Definition: PreferencesDialog.cpp:3099
rtabmap::MainWindow::rtabmapGoalStatusEventReceived
void rtabmapGoalStatusEventReceived(int status)
rtabmap::Statistics::mapCorrection
const Transform & mapCorrection() const
Definition: Statistics.h:211
rtabmap::Statistics::localPath
const std::vector< int > & localPath() const
Definition: Statistics.h:218
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:68
rtabmap::MainWindow::drawKeypoints
void drawKeypoints(const std::multimap< int, cv::KeyPoint > &refWords, const std::multimap< int, cv::KeyPoint > &loopWords)
Definition: MainWindow.cpp:4109
rtabmap::MainWindow::_loopClosureIds
QVector< int > _loopClosureIds
Definition: MainWindow.h:379
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:116
rtabmap::SensorData::setDepthOrRightRaw
void setDepthOrRightRaw(const cv::Mat &depthOrImageRaw)
Definition: SensorData.h:165
rtabmap::PreferencesDialog::isCloudsShown
bool isCloudsShown(int index) const
Definition: PreferencesDialog.cpp:4483
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:61
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:206
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2039
rtabmap::PreferencesDialog::notifyWhenNewGlobalPathIsReceived
bool notifyWhenNewGlobalPathIsReceived() const
Definition: PreferencesDialog.cpp:4446
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::MainWindow::_exportCloudsDialog
ExportCloudsDialog * _exportCloudsDialog
Definition: MainWindow.h:308
rtabmap::Statistics::refImageId
int refImageId() const
Definition: Statistics.h:202
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:604
rtabmap::CloudViewer::getPose
bool getPose(const std::string &id, Transform &pose)
Definition: CloudViewer.cpp:2092
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
rtabmap::MainWindow::createAndAddFeaturesToMap
void createAndAddFeaturesToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:3444
rtabmap::PreferencesDialog::getScanColorScheme
int getScanColorScheme(int index) const
Definition: PreferencesDialog.cpp:4669
octomap::OcTreeBaseImpl::size
virtual size_t size() const
rtabmap::PreferencesDialog::getTmpIniFilePath
virtual QString getTmpIniFilePath() const
Definition: PreferencesDialog.cpp:1838
rtabmap::MainWindow::_lastOdomPose
Transform _lastOdomPose
Definition: MainWindow.h:354
rtabmap::MainWindow::_dataRecorder
DataRecorder * _dataRecorder
Definition: MainWindow.h:312
rtabmap::OccupancyGrid::addedNodes
const std::map< int, Transform > & addedNodes() const
Definition: OccupancyGrid.h:65
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:67
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
rtabmap::MainWindow::_loopClosureViewer
LoopClosureViewer * _loopClosureViewer
Definition: MainWindow.h:368
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::MainWindow::newDatabase
virtual void newDatabase()
Definition: MainWindow.cpp:4467
rtabmap::MainWindow::processRtabmapGlobalPathEvent
void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &event)
Definition: MainWindow.cpp:3912
rtabmap::MainWindow::updateCacheFromDatabase
void updateCacheFromDatabase()
Definition: MainWindow.cpp:6140
rtabmap::MainWindow::cloudViewer
rtabmap::CloudViewer * cloudViewer() const
Definition: MainWindow.h:288
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:828
rtabmap::CloudViewer::updateCloudPose
bool updateCloudPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:427
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
rtabmap::MainWindow::odometryReceived
void odometryReceived(const rtabmap::OdometryEvent &, bool)
rtabmap::MainWindow::downloadAllClouds
virtual void downloadAllClouds()
Definition: MainWindow.cpp:6185
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:2582
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:68
rtabmap::PreferencesDialog::getCloudMinDepth
double getCloudMinDepth(int index) const
Definition: PreferencesDialog.cpp:4605
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::CameraInfo::timeUndistortDepth
float timeUndistortDepth
Definition: CameraInfo.h:65
glm::rotate
GLM_FUNC_DECL detail::tmat4x4< T, P > rotate(detail::tmat4x4< T, P > const &m, T const &angle, detail::tvec3< T, P > const &axis)
rtabmap::MainWindow::selectScreenCaptureFormat
void selectScreenCaptureFormat(bool checked)
Definition: MainWindow.cpp:6458
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
util3d_surface.h
rtabmap::SensorData::setLaserScanRaw
void setLaserScanRaw(const LaserScan &laserScanRaw)
Definition: SensorData.h:166
rtabmap::Statistics::rawLikelihood
const std::map< int, float > & rawLikelihood() const
Definition: Statistics.h:217
rtabmap::MainWindow::processRtabmapGoalStatusEvent
void processRtabmapGoalStatusEvent(int status)
Definition: MainWindow.cpp:3985
rtabmap::MainWindow::_processingDownloadedMap
bool _processingDownloadedMap
Definition: MainWindow.h:318
rtabmap::RtabmapEvent3DMap::getSignatures
const std::map< int, Signature > & getSignatures() const
Definition: RtabmapEvent.h:185
rtabmap::Statistics::stamp
double stamp() const
Definition: Statistics.h:205
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31