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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59