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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29