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/Lidar.h"
35 #include "rtabmap/core/IMUThread.h"
36 #include "rtabmap/core/DBReader.h"
37 #include "rtabmap/core/Parameters.h"
38 #include "rtabmap/core/ParamEvent.h"
39 #include "rtabmap/core/Signature.h"
40 #include "rtabmap/core/Memory.h"
41 #include "rtabmap/core/DBDriver.h"
42 #include "rtabmap/core/RegistrationVis.h"
43 #include "rtabmap/core/global_map/OccupancyGrid.h"
44 #include "rtabmap/core/GainCompensator.h"
45 #include "rtabmap/core/Recovery.h"
46 #include "rtabmap/core/util2d.h"
47 
48 #include "rtabmap/gui/ImageView.h"
49 #include "rtabmap/gui/KeypointItem.h"
50 #include "rtabmap/gui/DataRecorder.h"
51 #include "rtabmap/gui/DatabaseViewer.h"
52 #include "rtabmap/gui/PdfPlot.h"
53 #include "rtabmap/gui/StatsToolBox.h"
54 #include "rtabmap/gui/ProgressDialog.h"
55 #include "rtabmap/gui/CloudViewer.h"
56 #include "rtabmap/gui/LoopClosureViewer.h"
57 #include "rtabmap/gui/ExportCloudsDialog.h"
58 #include "rtabmap/gui/ExportBundlerDialog.h"
59 #include "rtabmap/gui/AboutDialog.h"
60 #include "rtabmap/gui/PostProcessingDialog.h"
61 #include "rtabmap/gui/DepthCalibrationDialog.h"
62 #include "rtabmap/gui/RecoveryState.h"
63 #include "rtabmap/gui/MultiSessionLocWidget.h"
64 
65 #include <rtabmap/utilite/UStl.h>
66 #include <rtabmap/utilite/ULogger.h>
67 #include <rtabmap/utilite/UEventsManager.h>
68 #include <rtabmap/utilite/UFile.h>
69 #include <rtabmap/utilite/UConversion.h>
70 #include "rtabmap/utilite/UPlot.h"
71 #include "rtabmap/utilite/UCv2Qt.h"
72 
73 #include <QtGui/QCloseEvent>
74 #include <QtGui/QPixmap>
75 #include <QtCore/QDir>
76 #include <QtCore/QFile>
77 #include <QtCore/QTextStream>
78 #include <QtCore/QFileInfo>
79 #include <QMessageBox>
80 #include <QFileDialog>
81 #include <QGraphicsEllipseItem>
82 #include <QDockWidget>
83 #include <QtCore/QBuffer>
84 #include <QtCore/QTimer>
85 #include <QtCore/QTime>
86 #include <QActionGroup>
87 #include <QtGui/QDesktopServices>
88 #include <QtCore/QStringList>
89 #include <QtCore/QProcess>
90 #include <QSplashScreen>
91 #include <QInputDialog>
92 #include <QToolButton>
93 
94 //RGB-D stuff
95 #include "rtabmap/core/CameraRGBD.h"
96 #include "rtabmap/core/Odometry.h"
97 #include "rtabmap/core/OdometryThread.h"
98 #include "rtabmap/core/OdometryEvent.h"
99 #include "rtabmap/core/util3d.h"
100 #include "rtabmap/core/util3d_transforms.h"
101 #include "rtabmap/core/util3d_filtering.h"
102 #include "rtabmap/core/util3d_mapping.h"
103 #include "rtabmap/core/util3d_surface.h"
104 #include "rtabmap/core/util3d_registration.h"
105 #include "rtabmap/core/optimizer/OptimizerCVSBA.h"
106 #include "rtabmap/core/Graph.h"
107 #include "rtabmap/core/RegistrationIcp.h"
108 #include <pcl/visualization/cloud_viewer.h>
109 #include <pcl/common/transforms.h>
110 #include <pcl/common/common.h>
111 #include <pcl/io/pcd_io.h>
112 #include <pcl/io/ply_io.h>
113 #include <pcl/filters/filter.h>
114 #include <pcl/search/kdtree.h>
115 #include <rtabmap/core/SensorCaptureThread.h>
116 #include <rtabmap/core/SensorEvent.h>
117 
118 #ifdef RTABMAP_OCTOMAP
119 #include <rtabmap/core/global_map/OctoMap.h>
120 #endif
121 
122 #ifdef RTABMAP_GRIDMAP
123 #include <rtabmap/core/global_map/GridMap.h>
124 #endif
125 
126 #ifdef HAVE_OPENCV_ARUCO
127 #include <opencv2/aruco.hpp>
128 #endif
129 
130 #define LOG_FILE_NAME "LogRtabmap.txt"
131 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
132 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
133 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m"
134 
135 using namespace rtabmap;
136 
137 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
138 
139 
140 namespace rtabmap {
141 
142 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool showSplashScreen) :
143  QMainWindow(parent),
144  _ui(0),
145  _state(kIdle),
146  _sensorCapture(0),
147  _odomThread(0),
148  _imuThread(0),
149  _preferencesDialog(0),
150  _aboutDialog(0),
151  _exportCloudsDialog(0),
152  _exportBundlerDialog(0),
153  _dataRecorder(0),
154  _lastId(0),
155  _firstStamp(0.0f),
156  _processingStatistics(false),
157  _processingDownloadedMap(false),
158  _recovering(false),
159  _odometryReceived(false),
160  _newDatabasePath(""),
161  _newDatabasePathOutput(""),
162  _openedDatabasePath(""),
163  _databaseUpdated(false),
164  _odomImageShow(true),
165  _odomImageDepthShow(false),
166  _savedMaximized(false),
167  _waypointsIndex(0),
168  _cachedMemoryUsage(0),
169  _createdCloudsMemoryUsage(0),
170  _occupancyGrid(0),
171  _octomap(0),
172  _elevationMap(0),
173  _odometryCorrection(Transform::getIdentity()),
174  _processingOdometry(false),
175  _oneSecondTimer(0),
176  _elapsedTime(0),
177  _logEventTime(0),
178  _posteriorCurve(0),
179  _likelihoodCurve(0),
180  _rawLikelihoodCurve(0),
181  _exportPosesFrame(0),
182  _autoScreenCaptureOdomSync(false),
183  _autoScreenCaptureRAM(false),
184  _autoScreenCapturePNG(false),
185  _firstCall(true),
186  _progressCanceled(false)
187 {
188  ULogger::registerCurrentThread("MainWindow");
189  UDEBUG("");
190 
191  initGuiResource();
192 
193  QSplashScreen * splash = 0;
194  if (showSplashScreen)
195  {
196  QPixmap pixmap(":images/RTAB-Map.png");
197  splash = new QSplashScreen(pixmap);
198  splash->show();
199  splash->showMessage(tr("Loading..."));
200  QApplication::processEvents();
201  }
202 
203  // Create dialogs
204  _aboutDialog = new AboutDialog(this);
205  _aboutDialog->setObjectName("AboutDialog");
206  _exportCloudsDialog = new ExportCloudsDialog(this);
207  _exportCloudsDialog->setObjectName("ExportCloudsDialog");
208  _exportBundlerDialog = new ExportBundlerDialog(this);
209  _exportBundlerDialog->setObjectName("ExportBundlerDialog");
210  _postProcessingDialog = new PostProcessingDialog(this);
211  _postProcessingDialog->setObjectName("PostProcessingDialog");
212  _depthCalibrationDialog = new DepthCalibrationDialog(this);
213  _depthCalibrationDialog->setObjectName("DepthCalibrationDialog");
214 
215  _ui = new Ui_mainWindow();
216  UDEBUG("Setup ui...");
217  _ui->setupUi(this);
218 
219  // Add cloud viewers
220  // Note that we add them here manually because there is a crash issue
221  // when adding them in a DockWidget of the *.ui file. The cloud viewer is
222  // created in a widget which is not yet linked to main window when the CloudViewer constructor
223  // is called (see order in generated ui file). VTK needs to get the top
224  // level window at the time CloudViewer is created, otherwise it may crash on some systems.
225  _cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
226  _cloudViewer->setObjectName("widget_cloudViewer");
227  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
228  _loopClosureViewer = new LoopClosureViewer(_ui->layout_loopClosureViewer);
229  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
230  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
231  UDEBUG("Setup ui... end");
232 
233  QString title("RTAB-Map[*]");
234  this->setWindowTitle(title);
235  this->setWindowIconText(tr("RTAB-Map"));
236  this->setObjectName("MainWindow");
237 
238  //Setup dock widgets position if it is the first time the application is started.
239  setDefaultViews();
240 
241  _ui->widget_mainWindow->setVisible(false);
242 
243  if(prefDialog)
244  {
245  _preferencesDialog = prefDialog;
246  _preferencesDialog->setParent(this, Qt::Dialog);
247  }
248  else // Default dialog
249  {
250  _preferencesDialog = new PreferencesDialog(this);
251  }
252 
253  _preferencesDialog->setObjectName("PreferencesDialog");
254  _preferencesDialog->init();
255 
256  // Restore window geometry
257  bool statusBarShown = false;
258  _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown);
259  _preferencesDialog->loadWindowGeometry(_preferencesDialog);
260  _preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
261  _preferencesDialog->loadWindowGeometry(_exportBundlerDialog);
262  _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
263  _preferencesDialog->loadWindowGeometry(_depthCalibrationDialog);
264  _preferencesDialog->loadWindowGeometry(_aboutDialog);
265  setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
266 
267  ParametersMap parameters = _preferencesDialog->getAllParameters();
268  // Override map resolution for UI
269  if(_preferencesDialog->getGridUIResolution()>0)
270  {
271  uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution())));
272  }
273  _occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters);
274 #ifdef RTABMAP_OCTOMAP
275  _octomap = new OctoMap(&_cachedLocalMaps, parameters);
276 #endif
277 #ifdef RTABMAP_GRIDMAP
278  _elevationMap = new GridMap(&_cachedLocalMaps, parameters);
279 #endif
280 
281  // Timer
282  _oneSecondTimer = new QTimer(this);
283  _oneSecondTimer->setInterval(1000);
284  _elapsedTime = new QElapsedTimer();
285  _ui->label_elapsedTime->setText("00:00:00");
286  connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
287  _logEventTime = new QElapsedTimer();
288  _logEventTime->start();
289 
290  //Graphics scenes
291  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
292  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
293  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
294  _ui->imageView_odometry->setAlpha(200);
295  _preferencesDialog->loadWidgetState(_ui->imageView_source);
296  _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
297  _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
298  _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
299 
300  _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
301  _ui->posteriorPlot->addCurve(_posteriorCurve, false);
302  _ui->posteriorPlot->showLegend(false);
303  _ui->posteriorPlot->setFixedYAxis(0,1);
304  UPlotCurveThreshold * tc;
305  tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
306  connect(this, SIGNAL(loopClosureThrChanged(qreal)), tc, SLOT(setThreshold(qreal)));
307 
308  _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
309  _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
310  _ui->likelihoodPlot->showLegend(false);
311 
312  _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
313  _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
314  _ui->rawLikelihoodPlot->showLegend(false);
315 
316  _multiSessionLocWidget = new MultiSessionLocWidget(&_cachedSignatures, &_currentMapIds, this);
317  _ui->layout_multiSessionLoc->layout()->addWidget(_multiSessionLocWidget);
318 
319  _progressDialog = new ProgressDialog(this);
320  _progressDialog->setMinimumWidth(800);
321  connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancelProgress()));
322 
323  connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
324 
325  //connect stuff
326  connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
327  qRegisterMetaType<MainWindow::State>("MainWindow::State");
328  connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
329  connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
330  qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
331  connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
332  qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
333  connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
334  connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
335  connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
336 
337  // Dock Widget view actions (Menu->Window)
338  _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
339  _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
340  _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
341  _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
342  _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
343  _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
344  _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
345  _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
346  _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
347  _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
348  _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
349  _ui->menuShow_view->addAction(_ui->dockWidget_multiSessionLoc->toggleViewAction());
350  _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
351  _ui->toolBar->setWindowTitle(tr("File toolbar"));
352  _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
353  _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
354  QAction * a = _ui->menuShow_view->addAction("Progress dialog");
355  a->setCheckable(false);
356  connect(a, SIGNAL(triggered(bool)), _progressDialog, SLOT(show()));
357  QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar");
358  statusBarAction->setCheckable(true);
359  statusBarAction->setChecked(statusBarShown);
360  connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool)));
361 
362  // connect actions with custom slots
363  connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
364  connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
365  connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
366  connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
367  connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
368  connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
369  connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
370  connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
371  connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
372  connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
373  connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
374  connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
375  connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
376  connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
377  connect(_ui->actionRemove_label, SIGNAL(triggered()), this, SLOT(removeLabel()));
378  connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
379  connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
380  connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
381  connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
382  connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
383  connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
384  connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
385  connect(_ui->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
386  connect(_ui->actionRGBD_SLAM_ID_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMID()));
387  connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
388  connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
389  connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
390  connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
391  connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
392  connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
393  connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
394  connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
395  connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
396  connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
397  connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
398  connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
399  connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
400  connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
401  connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
402  connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
403  connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
404  connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
405  connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
406  connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
407  connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom()));
408  connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
409  connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
410  connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages()));
411  connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat()));
412  connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap()));
413  connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
414  connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
415  connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
416  connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
417  connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(showPostProcessingDialog()));
418  connect(_ui->actionDepth_Calibration, SIGNAL(triggered()), this, SLOT(depthCalibration()));
419 
420  _ui->actionPause->setShortcut(Qt::Key_Space);
421  _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
422  // Qt5 issue, we should explicitly add actions not in
423  // menu bar to have shortcut working
424  this->addAction(_ui->actionSave_GUI_config);
425  _ui->actionReset_Odometry->setEnabled(false);
426  _ui->actionPost_processing->setEnabled(false);
427  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
428 
429  QToolButton* toolButton = new QToolButton(this);
430  toolButton->setMenu(_ui->menuSelect_source);
431  toolButton->setPopupMode(QToolButton::InstantPopup);
432  toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
433  toolButton->setToolTip("Select sensor driver");
434  _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
435 
436 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
437  connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
438 #else
439  _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
440 #endif
441 
442  //Settings menu
443  connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
444  connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
445  connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
446  connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
447  connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
448  connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
449  connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
450  connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
451  connect(_ui->actionOpenNI2_orbbec, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
452  connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
453  connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
454  connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
455  connect(_ui->actionKinect_for_Azure, SIGNAL(triggered()), this, SLOT(selectK4A()));
456  connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
457  connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
458  connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
459  connect(_ui->actionRealSense2_D400, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
460  connect(_ui->actionRealSense2_L515, SIGNAL(triggered()), this, SLOT(selectRealSense2L515()));
461  connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
462  connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
463  connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
464  connect(_ui->actionZed_Open_Capture, SIGNAL(triggered()), this, SLOT(selectStereoZedOC()));
465  connect(_ui->actionStereoTara, SIGNAL(triggered()), this, SLOT(selectStereoTara()));
466  connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
467  connect(_ui->actionRealSense2_T265, SIGNAL(triggered()), this, SLOT(selectRealSense2Stereo()));
468  connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS()));
469  connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD()));
470  connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite()));
471  connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro()));
472  connect(_ui->actionXvisio_SeerSense, SIGNAL(triggered()), this, SLOT(selectXvisioSeerSense()));
473  connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16()));
474  _ui->actionFreenect->setEnabled(CameraFreenect::available());
475  _ui->actionOpenNI_PCL->setEnabled(CameraOpenni::available());
476  _ui->actionOpenNI_PCL_ASUS->setEnabled(CameraOpenni::available());
477  _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
478  _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
479  _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
480  _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
481  _ui->actionOpenNI2_orbbec->setEnabled(CameraOpenNI2::available());
482  _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
483  _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
484  _ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
485  _ui->actionKinect_for_Azure->setEnabled(CameraK4A::available());
486  _ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
487  _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
488  _ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
489  _ui->actionRealSense2_D400->setEnabled(CameraRealSense2::available());
490  _ui->actionRealSense2_L515->setEnabled(CameraRealSense2::available());
491  _ui->actionRealSense2_T265->setEnabled(CameraRealSense2::available());
492  _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
493  _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
494  _ui->actionStereoZed->setEnabled(CameraStereoZed::available());
495  _ui->actionZed_Open_Capture->setEnabled(CameraStereoZedOC::available());
496  _ui->actionStereoTara->setEnabled(CameraStereoTara::available());
497  _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available());
498  _ui->actionDepthAI_oakd->setEnabled(CameraDepthAI::available());
499  _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available());
500  _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available());
501  _ui->actionXvisio_SeerSense->setEnabled(CameraSeerSense::available());
502  this->updateSelectSourceMenu();
503 
504  connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
505 
506  QActionGroup * modeGrp = new QActionGroup(this);
507  modeGrp->addAction(_ui->actionSLAM_mode);
508  modeGrp->addAction(_ui->actionLocalization_mode);
509  _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
510  _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
511  connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
512  connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
513  connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
514 
515  // Settings changed
516  qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
517  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
518  qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
519  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
520  // config GUI modified
521  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
522  if(prefDialog == 0)
523  {
524  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
525  }
526  connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
527  connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
528  connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
529  connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
530  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
531  connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
532  connect(_exportBundlerDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
533  connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
534  connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
535  connect(_multiSessionLocWidget->getImageView(), SIGNAL(configChanged()), this, SLOT(configGUIModified()));
536  connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
537  connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
538  connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
539  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
540  for(int i=0; i<dockWidgets.size(); ++i)
541  {
542  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
543  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
544  }
545  connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
546  // catch resize events
547  _ui->dockWidget_posterior->installEventFilter(this);
548  _ui->dockWidget_likelihood->installEventFilter(this);
549  _ui->dockWidget_rawlikelihood->installEventFilter(this);
550  _ui->dockWidget_statsV2->installEventFilter(this);
551  _ui->dockWidget_console->installEventFilter(this);
552  _ui->dockWidget_loopClosureViewer->installEventFilter(this);
553  _ui->dockWidget_mapVisibility->installEventFilter(this);
554  _ui->dockWidget_graphViewer->installEventFilter(this);
555  _ui->dockWidget_odometry->installEventFilter(this);
556  _ui->dockWidget_cloudViewer->installEventFilter(this);
557  _ui->dockWidget_imageView->installEventFilter(this);
558  _ui->dockWidget_multiSessionLoc->installEventFilter(this);
559 
560  // more connects...
561  _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
562  _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
563  _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
564  connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
565  connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
566  connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
567  connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
568  connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
569  connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
570 
571  // Statistics from the detector
572  qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
573  connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
574 
575  qRegisterMetaType<rtabmap::SensorCaptureInfo>("rtabmap::SensorCaptureInfo");
576  connect(this, SIGNAL(cameraInfoReceived(rtabmap::SensorCaptureInfo)), this, SLOT(processCameraInfo(rtabmap::SensorCaptureInfo)));
577 
578  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
579  connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool)));
580 
581  connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
582 
583  // Apply state
584  this->changeState(kIdle);
585  this->applyPrefSettings(PreferencesDialog::kPanelAll);
586 
587  _ui->statsToolBox->setNewFigureMaxItems(50);
588  _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
589  _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
590  _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
591  _cloudViewer->setBackfaceCulling(true, false);
592  _preferencesDialog->loadWidgetState(_cloudViewer);
593  _preferencesDialog->loadWidgetState(_multiSessionLocWidget->getImageView());
594 
595  //dialog states
596  _preferencesDialog->loadWidgetState(_exportCloudsDialog);
597  _preferencesDialog->loadWidgetState(_exportBundlerDialog);
598  _preferencesDialog->loadWidgetState(_postProcessingDialog);
599  _preferencesDialog->loadWidgetState(_depthCalibrationDialog);
600 
601  if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
602  {
603  const std::map<std::string, float> & statistics = Statistics::defaultData();
604  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
605  {
606  // Don't add Gt panels yet if we don't know if we will receive Gt values.
607  if(!QString((*iter).first.c_str()).contains("Gt/"))
608  {
609  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
610  }
611  }
612  }
613  // Specific MainWindow
614  _ui->statsToolBox->updateStat("Planning/From/", false);
615  _ui->statsToolBox->updateStat("Planning/Time/ms", false);
616  _ui->statsToolBox->updateStat("Planning/Goal/", false);
617  _ui->statsToolBox->updateStat("Planning/Poses/", false);
618  _ui->statsToolBox->updateStat("Planning/Length/m", false);
619 
620  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false);
621  _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", false);
622  _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", false);
623  _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", false);
624  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false);
625  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false);
626  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false);
627  _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", false);
628  _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", false);
629  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false);
630  _ui->statsToolBox->updateStat("Camera/Time total/ms", false);
631 
632  _ui->statsToolBox->updateStat("Odometry/ID/", false);
633  _ui->statsToolBox->updateStat("Odometry/Features/", false);
634  _ui->statsToolBox->updateStat("Odometry/Matches/", false);
635  _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
636  _ui->statsToolBox->updateStat("Odometry/Inliers/", false);
637  _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", false);
638  _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", false);
639  _ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
640  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
641  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
642  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
643  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
644  _ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
645  _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
646  _ui->statsToolBox->updateStat("Odometry/ICPRMS/", false);
647  _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", false);
648  _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", false);
649  _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
650  _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
651  _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", false);
652  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
653  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
654  _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false);
655  _ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", false);
656  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
657  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
658  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
659  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
660  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
661  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
662  _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
663  _ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
664  _ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
665  _ui->statsToolBox->updateStat("Odometry/Speed/mph", false);
666  _ui->statsToolBox->updateStat("Odometry/Speed/mps", false);
667  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", false);
668  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", false);
669  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", false);
670  _ui->statsToolBox->updateStat("Odometry/Distance/m", false);
671  _ui->statsToolBox->updateStat("Odometry/T/m", false);
672  _ui->statsToolBox->updateStat("Odometry/Tx/m", false);
673  _ui->statsToolBox->updateStat("Odometry/Ty/m", false);
674  _ui->statsToolBox->updateStat("Odometry/Tz/m", false);
675  _ui->statsToolBox->updateStat("Odometry/Troll/deg", false);
676  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", false);
677  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", false);
678  _ui->statsToolBox->updateStat("Odometry/Px/m", false);
679  _ui->statsToolBox->updateStat("Odometry/Py/m", false);
680  _ui->statsToolBox->updateStat("Odometry/Pz/m", false);
681  _ui->statsToolBox->updateStat("Odometry/Proll/deg", false);
682  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", false);
683  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", false);
684 
685  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false);
686  _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false);
687  _ui->statsToolBox->updateStat("GUI/Graph Update/ms", false);
688 #ifdef RTABMAP_OCTOMAP
689  _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
690  _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
691 #endif
692 #ifdef RTABMAP_GRIDMAP
693  _ui->statsToolBox->updateStat("GUI/Elevation Update/ms", false);
694  _ui->statsToolBox->updateStat("GUI/Elevation Rendering/ms", false);
695 #endif
696  _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
697  _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
698  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
699  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
700  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
701 #ifdef RTABMAP_OCTOMAP
702  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false);
703 #endif
704 
705  this->loadFigures();
706  connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
707 
708  // update loop closure viewer parameters
709  _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
710  _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
711 
712  if (splash)
713  {
714  splash->close();
715  delete splash;
716  }
717 
718  this->setFocus();
719 
720  UDEBUG("");
721 }
722 
723 MainWindow::~MainWindow()
724 {
725  UDEBUG("");
726  this->stopDetection();
727  delete _ui;
728  delete _elapsedTime;
729  delete _logEventTime;
730 #ifdef RTABMAP_OCTOMAP
731  delete _octomap;
732 #endif
733 #ifdef RTABMAP_GRIDMAP
734  delete _elevationMap;
735 #endif
736  delete _occupancyGrid;
737  UDEBUG("");
738 }
739 
740 void MainWindow::setupMainLayout(bool vertical)
741 {
742  if(vertical)
743  {
744  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
745  }
746  else if(!vertical)
747  {
748  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
749  }
750 }
751 
752 std::map<int, Transform> MainWindow::currentVisiblePosesMap() const
753 {
754  return _ui->widget_mapVisibility->getVisiblePoses();
755 }
756 
757 void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
758 {
759  UASSERT(cloudViewer);
760  delete _cloudViewer;
761  _cloudViewer = cloudViewer;
762  _cloudViewer->setParent(_ui->layout_cloudViewer);
763  _cloudViewer->setObjectName("widget_cloudViewer");
764  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
765 
766  _cloudViewer->setBackfaceCulling(true, false);
767  _preferencesDialog->loadWidgetState(_cloudViewer);
768 
769  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
770 }
771 void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
772 {
773  UASSERT(loopClosureViewer);
774  delete _loopClosureViewer;
775  _loopClosureViewer = loopClosureViewer;
776  _loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
777  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
778  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
779 }
780 
781 void MainWindow::closeEvent(QCloseEvent* event)
782 {
783  // Try to close all children
784  /*QList<QMainWindow *> windows = this->findChildren<QMainWindow *>();
785  for(int i=0; i<windows.size(); i++) {
786  if(!windows[i]->close()) {
787  event->setAccepted(false);
788  return;
789  }
790  }*/
791  UDEBUG("");
792  bool processStopped = true;
793  if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
794  {
795  this->stopDetection();
796  if(_state == kInitialized)
797  {
798  if(this->closeDatabase())
799  {
800  this->changeState(kApplicationClosing);
801  }
802  }
803  if(_state != kIdle)
804  {
805  processStopped = false;
806  }
807  }
808 
809  if(processStopped)
810  {
811  //write settings before quit?
812  bool save = false;
813  if(this->isWindowModified())
814  {
815  QMessageBox::Button b=QMessageBox::question(this,
816  tr("RTAB-Map"),
817  tr("There are unsaved changed settings. Save them?"),
818  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
819  if(b == QMessageBox::Save)
820  {
821  save = true;
822  }
823  else if(b != QMessageBox::Discard)
824  {
825  event->ignore();
826  return;
827  }
828  }
829 
830  if(save)
831  {
832  saveConfigGUI();
833  }
834 
835  _ui->statsToolBox->closeFigures();
836 
837  _ui->dockWidget_imageView->close();
838  _ui->dockWidget_likelihood->close();
839  _ui->dockWidget_rawlikelihood->close();
840  _ui->dockWidget_posterior->close();
841  _ui->dockWidget_statsV2->close();
842  _ui->dockWidget_console->close();
843  _ui->dockWidget_cloudViewer->close();
844  _ui->dockWidget_loopClosureViewer->close();
845  _ui->dockWidget_mapVisibility->close();
846  _ui->dockWidget_graphViewer->close();
847  _ui->dockWidget_odometry->close();
848  _ui->dockWidget_multiSessionLoc->close();
849 
850  if(_sensorCapture)
851  {
852  UERROR("Camera must be already deleted here!");
853  delete _sensorCapture;
854  _sensorCapture = 0;
855  if(_imuThread)
856  {
857  delete _imuThread;
858  _imuThread = 0;
859  }
860  }
861  if(_odomThread)
862  {
863  UERROR("OdomThread must be already deleted here!");
864  delete _odomThread;
865  _odomThread = 0;
866  }
867  event->accept();
868  }
869  else
870  {
871  event->ignore();
872  }
873  UDEBUG("");
874 }
875 
876 bool MainWindow::handleEvent(UEvent* anEvent)
877 {
878  if(anEvent->getClassName().compare("IMUEvent") == 0)
879  {
880  // IMU events are published at high frequency, early exit
881  return false;
882  }
883  else if(anEvent->getClassName().compare("RtabmapEvent") == 0)
884  {
885  RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
886  Statistics stats = rtabmapEvent->getStats();
887  int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
888  int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
889  bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
890  float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
891  if((stats.loopClosureId() > 0 &&
892  _ui->actionPause_on_match->isChecked())
893  ||
894  (stats.loopClosureId() == 0 &&
895  highestHypothesisId > 0 &&
896  highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
897  _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
898  rejectedHyp)
899  ||
900  (proximityClosureId > 0 &&
901  _ui->actionPause_on_local_loop_detection->isChecked()))
902  {
903  if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
904  {
905  if(_preferencesDialog->beepOnPause())
906  {
907  QMetaObject::invokeMethod(this, "beep");
908  }
909  this->pauseDetection();
910  }
911  }
912 
913  if(!_processingDownloadedMap)
914  {
915  _processingStatistics = true;
916  Q_EMIT statsReceived(stats);
917  }
918  }
919  else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
920  {
921  if(!_recovering)
922  {
923  RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
924  Q_EMIT rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
925  }
926  }
927  else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
928  {
929  RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
930  Q_EMIT rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
931  }
932  else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
933  {
934  RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
935  Q_EMIT rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
936  }
937  else if(anEvent->getClassName().compare("RtabmapLabelErrorEvent") == 0)
938  {
939  RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
940  Q_EMIT rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
941  }
942  else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
943  {
944  Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode());
945  }
946  else if(anEvent->getClassName().compare("SensorEvent") == 0)
947  {
948  SensorEvent * sensorEvent = (SensorEvent*)anEvent;
949  if(sensorEvent->getCode() == SensorEvent::kCodeNoMoreImages)
950  {
951  if(_preferencesDialog->beepOnPause())
952  {
953  QMetaObject::invokeMethod(this, "beep");
954  }
955  Q_EMIT noMoreImagesReceived();
956  }
957  else
958  {
959  Q_EMIT cameraInfoReceived(sensorEvent->info());
960  if (_odomThread == 0 && (_sensorCapture->odomProvided()) && _preferencesDialog->isRGBDMode())
961  {
962  OdometryInfo odomInfo;
963  odomInfo.reg.covariance = sensorEvent->info().odomCovariance;
964  if (!_processingOdometry && !_processingStatistics)
965  {
966  _processingOdometry = true; // if we receive too many odometry events!
967  OdometryEvent tmp(sensorEvent->data(), sensorEvent->info().odomPose, odomInfo);
968  Q_EMIT odometryReceived(tmp, false);
969  }
970  else
971  {
972  // we receive too many odometry events! ignore them
973  }
974  }
975  }
976  }
977  else if(anEvent->getClassName().compare("OdometryEvent") == 0)
978  {
979  OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
980  if(!_processingOdometry && !_processingStatistics)
981  {
982  _processingOdometry = true; // if we receive too many odometry events!
983  Q_EMIT odometryReceived(*odomEvent, false);
984  }
985  else
986  {
987  // we receive too many odometry events! just send without data
988  SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
989  data.setCameraModels(odomEvent->data().cameraModels());
990  data.setStereoCameraModels(odomEvent->data().stereoCameraModels());
991  data.setGroundTruth(odomEvent->data().groundTruth());
992  OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
993  Q_EMIT odometryReceived(tmp, true);
994  }
995  }
996  else if(anEvent->getClassName().compare("ULogEvent") == 0)
997  {
998  ULogEvent * logEvent = (ULogEvent*)anEvent;
999  if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
1000  {
1001  QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
1002  // The timer prevents multiple calls to pauseDetection() before the state can be changed
1003  if(_state != kPaused && _state != kMonitoringPaused && _state != kMonitoring && _logEventTime->elapsed() > 1000)
1004  {
1005  _logEventTime->start();
1006  if(_preferencesDialog->beepOnPause())
1007  {
1008  QMetaObject::invokeMethod(this, "beep");
1009  }
1010  pauseDetection();
1011  }
1012  }
1013  }
1014  return false;
1015 }
1016 
1017 void MainWindow::processCameraInfo(const rtabmap::SensorCaptureInfo & info)
1018 {
1019  if(_firstStamp == 0.0)
1020  {
1021  _firstStamp = info.stamp;
1022  }
1023  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
1024  {
1025  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1026  _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1027  _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1028  _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1029  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1030  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1031  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1032  _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeHistogramEqualization*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1033  _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1034  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1035  _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1036  }
1037 
1038  Q_EMIT(cameraInfoProcessed());
1039 }
1040 
1041 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
1042 {
1043  if(_firstStamp == 0.0)
1044  {
1045  _firstStamp = odom.data().stamp();
1046  }
1047 
1048  UDEBUG("");
1049  _processingOdometry = true;
1050  UTimer time;
1051  UTimer timeTotal;
1052  // Process Data
1053 
1054  // Set color code as tooltip
1055  if(_ui->imageView_odometry->toolTip().isEmpty())
1056  {
1057  _ui->imageView_odometry->setToolTip(
1058  "Background Color Code:\n"
1059  " Dark Red = Odometry Lost\n"
1060  " Dark Yellow = Low Inliers\n"
1061  "Feature Color code:\n"
1062  " Green = Inliers\n"
1063  " Yellow = Not matched features from previous frame(s)\n"
1064  " Red = Outliers");
1065  }
1066 
1067  Transform pose = odom.pose();
1068  bool lost = false;
1069  bool lostStateChanged = false;
1070 
1071  if(pose.isNull())
1072  {
1073  UDEBUG("odom lost"); // use last pose
1074  lostStateChanged = _cloudViewer->getBackgroundColor() != Qt::darkRed;
1075  _cloudViewer->setBackgroundColor(Qt::darkRed);
1076  _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
1077 
1078  pose = _lastOdomPose;
1079  lost = true;
1080  }
1081  else if(odom.info().reg.inliers>0 &&
1082  _preferencesDialog->getOdomQualityWarnThr() &&
1083  odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
1084  {
1085  UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
1086  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
1087  _cloudViewer->setBackgroundColor(Qt::darkYellow);
1088  _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
1089  }
1090  else
1091  {
1092  UDEBUG("odom ok");
1093  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
1094  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
1095  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
1096  }
1097 
1098  if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
1099  {
1100  _lastOdomPose = pose;
1101  }
1102 
1103  const SensorData * data = &odom.data();
1104 
1105  SensorData rectifiedData;
1106  if(!data->imageRaw().empty() &&
1107  ((_ui->dockWidget_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(1)) ||
1108  (_ui->dockWidget_odometry->isVisible() && (_ui->imageView_odometry->isImageShown() || _ui->imageView_odometry->isImageDepthShown()))))
1109  {
1110  // Do we need to rectify images?
1111  ParametersMap allParameters = _preferencesDialog->getAllParameters();
1112  bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
1113  Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
1114  if(!imagesAlreadyRectified)
1115  {
1116  rectifiedData = odom.data();
1117  if(data->cameraModels().size())
1118  {
1119  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
1120  UASSERT(int((data->imageRaw().cols/data->cameraModels().size())*data->cameraModels().size()) == data->imageRaw().cols);
1121  int subImageWidth = data->imageRaw().cols/data->cameraModels().size();
1122  cv::Mat rectifiedImages = data->imageRaw().clone();
1123  bool initRectMaps = _rectCameraModelsOdom.empty() || _rectCameraModelsOdom.size()!=data->cameraModels().size();
1124  if(initRectMaps)
1125  {
1126  _rectCameraModelsOdom.resize(data->cameraModels().size());
1127  }
1128  for(unsigned int i=0; i<data->cameraModels().size(); ++i)
1129  {
1130  if(data->cameraModels()[i].isValidForRectification())
1131  {
1132  if(initRectMaps)
1133  {
1134  _rectCameraModelsOdom[i] = data->cameraModels()[i];
1135  if(!_rectCameraModelsOdom[i].isRectificationMapInitialized())
1136  {
1137  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
1138  _rectCameraModelsOdom[i].initRectificationMap();
1139  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
1140  }
1141  }
1142  UASSERT(_rectCameraModelsOdom[i].imageWidth() == data->cameraModels()[i].imageWidth() &&
1143  _rectCameraModelsOdom[i].imageHeight() == data->cameraModels()[i].imageHeight());
1144  cv::Mat rectifiedImage = _rectCameraModelsOdom[i].rectifyImage(cv::Mat(data->imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
1145  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
1146  }
1147  else
1148  {
1149  UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
1150  i, data->id(),
1151  data->cameraModels()[i].imageWidth(),
1152  data->cameraModels()[i].imageHeight());
1153  }
1154  }
1155  rectifiedData.setRGBDImage(rectifiedImages, data->depthOrRightRaw(), data->cameraModels());
1156  }
1157  else if(!data->rightRaw().empty() && data->stereoCameraModels().size())
1158  {
1159  UASSERT(int((data->imageRaw().cols/data->stereoCameraModels().size())*data->stereoCameraModels().size()) == data->imageRaw().cols);
1160  int subImageWidth = data->imageRaw().cols/data->stereoCameraModels().size();
1161  cv::Mat rectifiedLeftImages = data->imageRaw().clone();
1162  cv::Mat rectifiedRightImages = data->imageRaw().clone();
1163  bool initRectMaps = _rectCameraModelsOdom.empty() || _rectCameraModelsOdom.size()!=data->stereoCameraModels().size()*2;
1164  if(initRectMaps)
1165  {
1166  _rectCameraModelsOdom.resize(data->stereoCameraModels().size()*2);
1167  }
1168  for(unsigned int i=0; i<data->stereoCameraModels().size(); ++i)
1169  {
1170  if(data->stereoCameraModels()[i].isValidForRectification())
1171  {
1172  if(initRectMaps)
1173  {
1174  _rectCameraModelsOdom[i*2] = data->stereoCameraModels()[i].left();
1175  _rectCameraModelsOdom[i*2+1] = data->stereoCameraModels()[i].right();
1176  if(!_rectCameraModelsOdom[i*2].isRectificationMapInitialized())
1177  {
1178  UWARN("Initializing rectification maps for stereo camera %d (only done for the first image received)...", i);
1179  _rectCameraModelsOdom[i*2].initRectificationMap();
1180  _rectCameraModelsOdom[i*2+1].initRectificationMap();
1181  UWARN("Initializing rectification maps for stereo camera %d (only done for the first image received)... done!", i);
1182  }
1183  }
1184  UASSERT(_rectCameraModelsOdom[i*2].imageWidth() == data->stereoCameraModels()[i].left().imageWidth() &&
1185  _rectCameraModelsOdom[i*2].imageHeight() == data->stereoCameraModels()[i].left().imageHeight() &&
1186  _rectCameraModelsOdom[i*2+1].imageWidth() == data->stereoCameraModels()[i].right().imageWidth() &&
1187  _rectCameraModelsOdom[i*2+1].imageHeight() == data->stereoCameraModels()[i].right().imageHeight());
1188  cv::Mat rectifiedLeftImage = _rectCameraModelsOdom[i*2].rectifyImage(cv::Mat(data->imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
1189  cv::Mat rectifiedRightImage = _rectCameraModelsOdom[i*2+1].rectifyImage(cv::Mat(data->rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data->rightRaw().rows)));
1190  rectifiedLeftImage.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
1191  rectifiedRightImage.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data->rightRaw().rows)));
1192  }
1193  else
1194  {
1195  UWARN("Stereo camera %d of data %d is not valid for rectification (%dx%d).",
1196  i, data->id(),
1197  data->stereoCameraModels()[i].left().imageWidth(),
1198  data->stereoCameraModels()[i].right().imageHeight());
1199  }
1200  }
1201  rectifiedData.setStereoImage(rectifiedLeftImages, rectifiedRightImages, data->stereoCameraModels());
1202  }
1203  UDEBUG("Time rectification: %fs", time.ticks());
1204  data = &rectifiedData;
1205  }
1206  }
1207 
1208  if(_ui->dockWidget_cloudViewer->isVisible())
1209  {
1210  bool cloudUpdated = false;
1211  bool scanUpdated = false;
1212  bool featuresUpdated = false;
1213  bool filteredGravityUpdated = false;
1214  bool accelerationUpdated = false;
1215  if(!pose.isNull())
1216  {
1217  // 3d cloud
1218  if(!data->imageRaw().empty() &&
1219  !data->depthOrRightRaw().empty() &&
1220  (data->cameraModels().size() || data->stereoCameraModels().size()) &&
1221  _preferencesDialog->isCloudsShown(1))
1222  {
1223  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1224  pcl::IndicesPtr indices(new std::vector<int>);
1225 
1226  cloud = util3d::cloudRGBFromSensorData(*data,
1227  _preferencesDialog->getCloudDecimation(1),
1228  _preferencesDialog->getCloudMaxDepth(1),
1229  _preferencesDialog->getCloudMinDepth(1),
1230  indices.get(),
1231  _preferencesDialog->getAllParameters(),
1232  _preferencesDialog->getCloudRoiRatios(1));
1233  if(indices->size())
1234  {
1235  cloud = util3d::transformPointCloud(cloud, pose);
1236 
1237  if(_preferencesDialog->isCloudMeshing())
1238  {
1239  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
1240  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
1241  output = util3d::extractIndices(cloud, indices, false, true);
1242 
1243  // Fast organized mesh
1244  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
1245  if(data->cameraModels().size() && !data->cameraModels()[0].localTransform().isNull())
1246  {
1247  viewpoint[0] = data->cameraModels()[0].localTransform().x();
1248  viewpoint[1] = data->cameraModels()[0].localTransform().y();
1249  viewpoint[2] = data->cameraModels()[0].localTransform().z();
1250  }
1251  else if(data->stereoCameraModels().size() && !data->stereoCameraModels()[0].localTransform().isNull())
1252  {
1253  viewpoint[0] = data->stereoCameraModels()[0].localTransform().x();
1254  viewpoint[1] = data->stereoCameraModels()[0].localTransform().y();
1255  viewpoint[2] = data->stereoCameraModels()[0].localTransform().z();
1256  }
1257  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
1258  output,
1259  _preferencesDialog->getCloudMeshingAngle(),
1260  _preferencesDialog->isCloudMeshingQuad(),
1261  _preferencesDialog->getCloudMeshingTriangleSize(),
1262  Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
1263  if(polygons.size())
1264  {
1265  if(_preferencesDialog->isCloudMeshingTexture() && !data->imageRaw().empty())
1266  {
1267  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
1268  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1269  textureMesh->tex_polygons.push_back(polygons);
1270  int w = cloud->width;
1271  int h = cloud->height;
1272  UASSERT(w > 1 && h > 1);
1273  textureMesh->tex_coordinates.resize(1);
1274  int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
1275  textureMesh->tex_coordinates[0].resize(nPoints);
1276  for(int i=0; i<nPoints; ++i)
1277  {
1278  //uv
1279  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
1280  float(i % w) / float(w), // u
1281  float(h - i / w) / float(h)); // v
1282  }
1283 
1284  pcl::TexMaterial mesh_material;
1285  mesh_material.tex_d = 1.0f;
1286  mesh_material.tex_Ns = 75.0f;
1287  mesh_material.tex_illum = 1;
1288 
1289  mesh_material.tex_name = "material_odom";
1290  mesh_material.tex_file = "";
1291  textureMesh->tex_materials.push_back(mesh_material);
1292 
1293  if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, data->imageRaw(), _odometryCorrection))
1294  {
1295  UERROR("Adding cloudOdom to viewer failed!");
1296  }
1297  }
1298  else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
1299  {
1300  UERROR("Adding cloudOdom to viewer failed!");
1301  }
1302  }
1303  }
1304  else
1305  {
1306  if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
1307  {
1308  UERROR("Adding cloudOdom to viewer failed!");
1309  }
1310  }
1311  _cloudViewer->setCloudVisibility("cloudOdom", true);
1312  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
1313  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
1314  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
1315 
1316  cloudUpdated = true;
1317  }
1318  }
1319 
1320  if(_preferencesDialog->isScansShown(1))
1321  {
1322  // F2M: scan local map
1323  if(!odom.info().localScanMap.isEmpty())
1324  {
1325  if(!lost)
1326  {
1327  bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanMapOdom");
1328  bool scanAdded = false;
1329  if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
1330  {
1331  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1332  util3d::laserScanToPointCloudINormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1333  _odometryCorrection, Qt::blue);
1334  }
1335  else if(odom.info().localScanMap.hasNormals())
1336  {
1337  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1338  util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1339  _odometryCorrection, Qt::blue);
1340  }
1341  else if(odom.info().localScanMap.hasIntensity())
1342  {
1343  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1344  util3d::laserScanToPointCloudI(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1345  _odometryCorrection, Qt::blue);
1346  }
1347  else
1348  {
1349  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1350  util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1351  _odometryCorrection, Qt::blue);
1352  }
1353 
1354 
1355  if(!scanAdded)
1356  {
1357  UERROR("Adding scanMapOdom to viewer failed!");
1358  }
1359  else
1360  {
1361  _cloudViewer->setCloudVisibility("scanMapOdom", true);
1362  _cloudViewer->setCloudColorIndex("scanMapOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1363  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
1364  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
1365  }
1366  }
1367  scanUpdated = true;
1368  }
1369  // scan cloud
1370  if(!data->laserScanRaw().isEmpty())
1371  {
1372  LaserScan scan = data->laserScanRaw();
1373 
1374  if(_preferencesDialog->getDownsamplingStepScan(1) > 1 ||
1375  _preferencesDialog->getScanMaxRange(1) > 0.0f ||
1376  _preferencesDialog->getScanMinRange(1) > 0.0f)
1377  {
1378  scan = util3d::commonFiltering(scan,
1379  _preferencesDialog->getDownsamplingStepScan(1),
1380  _preferencesDialog->getScanMinRange(1),
1381  _preferencesDialog->getScanMaxRange(1));
1382  }
1383  bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanOdom");
1384  bool scanAdded = false;
1385 
1386  if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
1387  {
1388  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud;
1389  cloud = util3d::laserScanToPointCloudINormal(scan, pose*scan.localTransform());
1390  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1391  {
1392  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1393  }
1394  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1395  }
1396  else if(odom.info().localScanMap.hasNormals())
1397  {
1398  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
1399  cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
1400  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1401  {
1402  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1403  }
1404  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1405  }
1406  else if(odom.info().localScanMap.hasIntensity())
1407  {
1408  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
1409  cloud = util3d::laserScanToPointCloudI(scan, pose*scan.localTransform());
1410  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1411  {
1412  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1413  }
1414  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1415  }
1416  else
1417  {
1418  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
1419  cloud = util3d::laserScanToPointCloud(scan, pose*scan.localTransform());
1420  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1421  {
1422  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1423  }
1424  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1425  }
1426 
1427  if(!scanAdded)
1428  {
1429  UERROR("Adding scanOdom to viewer failed!");
1430  }
1431  else
1432  {
1433  _cloudViewer->setCloudVisibility("scanOdom", true);
1434  _cloudViewer->setCloudColorIndex("scanOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1435  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
1436  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
1437  scanUpdated = true;
1438  }
1439  }
1440  }
1441 
1442  // 3d features
1443  if(_preferencesDialog->isFeaturesShown(1) && !odom.info().localMap.empty())
1444  {
1445  if(!lost)
1446  {
1447  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1448  cloud->resize(odom.info().localMap.size());
1449  int i=0;
1450  for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
1451  {
1452  // filter very far features from current location
1453  if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 100*100)
1454  {
1455  (*cloud)[i].x = iter->second.x;
1456  (*cloud)[i].y = iter->second.y;
1457  (*cloud)[i].z = iter->second.z;
1458 
1459  // green = inlier, yellow = outliers
1460  bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
1461  (*cloud)[i].r = inlier?0:255;
1462  (*cloud)[i].g = 255;
1463  (*cloud)[i].b = 0;
1464  if(!_preferencesDialog->isOdomOnlyInliersShown() || inlier)
1465  {
1466  ++i;
1467  }
1468  }
1469  }
1470  cloud->resize(i);
1471 
1472  _cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
1473  _cloudViewer->setCloudVisibility("featuresOdom", true);
1474  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
1475  }
1476  featuresUpdated = true;
1477  }
1478 
1479  if(_preferencesDialog->isFrustumsShown(1))
1480  {
1481  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
1482  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
1483  {
1484  std::list<std::string> splitted = uSplitNumChar(iter.key());
1485  if(splitted.size() == 2)
1486  {
1487  int id = std::atoi(splitted.back().c_str());
1488  id -= id%10;
1489  if(splitted.front().compare("f_odom_") == 0 &&
1490  odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
1491  {
1492  _cloudViewer->removeFrustum(iter.key());
1493  }
1494  }
1495  }
1496 
1497  for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
1498  {
1499  std::string frustumId = uFormat("f_odom_%d", iter->first*10);
1500  if(_cloudViewer->getAddedFrustums().contains(frustumId))
1501  {
1502  for(size_t i=0; i<10; ++i)
1503  {
1504  std::string subFrustumId = uFormat("f_odom_%d", iter->first*10+i);
1505  _cloudViewer->updateFrustumPose(subFrustumId, _odometryCorrection*iter->second);
1506  }
1507  }
1508  else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
1509  {
1510  const std::vector<CameraModel> & models = odom.info().localBundleModels.at(iter->first);
1511  for(size_t i=0; i<models.size(); ++i)
1512  {
1513  Transform t = models[i].localTransform();
1514  if(!t.isNull())
1515  {
1516  QColor color = Qt::yellow;
1517  std::string subFrustumId = uFormat("f_odom_%d", iter->first*10+i);
1518  _cloudViewer->addOrUpdateFrustum(subFrustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, models[i].fovX(), models[i].fovY());
1519  }
1520  }
1521  }
1522  }
1523  }
1524  if( _preferencesDialog->isIMUGravityShown(1) &&
1525  (data->imu().orientation().val[0]!=0 ||
1526  data->imu().orientation().val[1]!=0 ||
1527  data->imu().orientation().val[2]!=0 ||
1528  data->imu().orientation().val[3]!=0))
1529  {
1530  Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
1531  Transform orientation(0,0,0, data->imu().orientation()[0], data->imu().orientation()[1], data->imu().orientation()[2], data->imu().orientation()[3]);
1532  gravity = (orientation* data->imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
1533  _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);
1534  filteredGravityUpdated = true;
1535  }
1536  if( _preferencesDialog->isIMUAccShown() &&
1537  (data->imu().linearAcceleration().val[0]!=0 ||
1538  data->imu().linearAcceleration().val[1]!=0 ||
1539  data->imu().linearAcceleration().val[2]!=0))
1540  {
1541  Eigen::Vector3f gravity(
1542  -data->imu().linearAcceleration().val[0],
1543  -data->imu().linearAcceleration().val[1],
1544  -data->imu().linearAcceleration().val[2]);
1545  gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
1546  gravity = data->imu().localTransform().toEigen3f()*gravity;
1547  _cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
1548  accelerationUpdated = true;
1549  }
1550  }
1551  if(!dataIgnored)
1552  {
1553  if(!cloudUpdated && _cloudViewer->getAddedClouds().contains("cloudOdom"))
1554  {
1555  _cloudViewer->setCloudVisibility("cloudOdom", false);
1556  }
1557  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanOdom"))
1558  {
1559  _cloudViewer->setCloudVisibility("scanOdom", false);
1560  }
1561  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanMapOdom"))
1562  {
1563  _cloudViewer->setCloudVisibility("scanMapOdom", false);
1564  }
1565  if(!featuresUpdated && _cloudViewer->getAddedClouds().contains("featuresOdom"))
1566  {
1567  _cloudViewer->setCloudVisibility("featuresOdom", false);
1568  }
1569  if(!filteredGravityUpdated && _cloudViewer->getAddedLines().find("odom_imu_orientation") != _cloudViewer->getAddedLines().end())
1570  {
1571  _cloudViewer->removeLine("odom_imu_orientation");
1572  }
1573  if(!accelerationUpdated && _cloudViewer->getAddedLines().find("odom_imu_acc") != _cloudViewer->getAddedLines().end())
1574  {
1575  _cloudViewer->removeLine("odom_imu_acc");
1576  }
1577  }
1578  UDEBUG("Time 3D Rendering: %fs", time.ticks());
1579  }
1580 
1581  if(!odom.pose().isNull())
1582  {
1583  _odometryReceived = true;
1584  // update camera position
1585  if(data->cameraModels().size() && data->cameraModels()[0].isValidForProjection())
1586  {
1587  _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels());
1588  }
1589  else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection())
1590  {
1591  _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels());
1592  }
1593  else if(!data->laserScanRaw().isEmpty() ||
1594  !data->laserScanCompressed().isEmpty())
1595  {
1596  Transform scanLocalTransform;
1597  if(!data->laserScanRaw().isEmpty())
1598  {
1599  scanLocalTransform = data->laserScanRaw().localTransform();
1600  }
1601  else
1602  {
1603  scanLocalTransform = data->laserScanCompressed().localTransform();
1604  }
1605  //fake frustum
1606  CameraModel model(
1607  2,
1608  2,
1609  2,
1610  1.5,
1611  scanLocalTransform*CameraModel::opticalRotation(),
1612  0,
1613  cv::Size(4,3));
1614  _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model);
1615 
1616  }
1617 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
1618  if(_preferencesDialog->isFramesShown())
1619  {
1620  _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false);
1621  }
1622  else
1623  {
1624  _cloudViewer->removeLine("odom_to_base_link");
1625  }
1626 #endif
1627  _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
1628  UDEBUG("Time Update Pose: %fs", time.ticks());
1629  }
1630  _cloudViewer->refreshView();
1631 
1632  if(_ui->graphicsView_graphView->isVisible())
1633  {
1634  if(!pose.isNull() && !odom.pose().isNull())
1635  {
1636  _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
1637  _ui->graphicsView_graphView->update();
1638  UDEBUG("Time Update graphview: %fs", time.ticks());
1639  }
1640  }
1641 
1642  if(_ui->dockWidget_odometry->isVisible() &&
1643  !data->imageRaw().empty())
1644  {
1645  if(_ui->imageView_odometry->isFeaturesShown())
1646  {
1647  if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
1648  {
1649  if(_preferencesDialog->isOdomOnlyInliersShown())
1650  {
1651  std::multimap<int, cv::KeyPoint> kpInliers;
1652  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1653  {
1654  kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
1655  }
1656  _ui->imageView_odometry->setFeatures(
1657  kpInliers,
1658  data->depthRaw(),
1659  Qt::green);
1660  }
1661  else
1662  {
1663  _ui->imageView_odometry->setFeatures(
1664  odom.info().words,
1665  data->depthRaw(),
1666  Qt::yellow);
1667  }
1668  }
1669  else if(odom.info().type == (int)Odometry::kTypeF2F ||
1670  odom.info().type == (int)Odometry::kTypeViso2 ||
1671  odom.info().type == (int)Odometry::kTypeFovis ||
1672  odom.info().type == (int)Odometry::kTypeMSCKF ||
1673  odom.info().type == (int)Odometry::kTypeVINS ||
1674  odom.info().type == (int)Odometry::kTypeOpenVINS)
1675  {
1676  std::vector<cv::KeyPoint> kpts;
1677  cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
1678  _ui->imageView_odometry->setFeatures(
1679  kpts,
1680  data->depthRaw(),
1681  Qt::red);
1682  }
1683  }
1684 
1685  //detect if it is OdometryMono initialization
1686  bool monoInitialization = false;
1687  if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F)
1688  {
1689  monoInitialization = true;
1690  }
1691 
1692  _ui->imageView_odometry->clearLines();
1693  if(lost && !monoInitialization)
1694  {
1695  if(lostStateChanged)
1696  {
1697  // save state
1698  _odomImageShow = _ui->imageView_odometry->isImageShown();
1699  _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
1700  }
1701  _ui->imageView_odometry->setImageDepth(data->imageRaw());
1702  _ui->imageView_odometry->setImageShown(true);
1703  _ui->imageView_odometry->setImageDepthShown(true);
1704  }
1705  else
1706  {
1707  if(lostStateChanged)
1708  {
1709  // restore state
1710  _ui->imageView_odometry->setImageShown(_odomImageShow);
1711  _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
1712  }
1713 
1714  _ui->imageView_odometry->setImage(uCvMat2QImage(data->imageRaw()));
1715  if(_ui->imageView_odometry->isImageDepthShown() && !data->depthOrRightRaw().empty())
1716  {
1717  _ui->imageView_odometry->setImageDepth(data->depthOrRightRaw());
1718  }
1719 
1720  if( odom.info().type == (int)Odometry::kTypeF2M ||
1721  odom.info().type == (int)Odometry::kTypeORBSLAM ||
1722  odom.info().type == (int)Odometry::kTypeMSCKF ||
1723  odom.info().type == (int)Odometry::kTypeVINS ||
1724  odom.info().type == (int)Odometry::kTypeOpenVINS)
1725  {
1726  if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
1727  {
1728  for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
1729  {
1730  _ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
1731  }
1732  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1733  {
1734  _ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
1735  }
1736  }
1737  }
1738  if((odom.info().type == (int)Odometry::kTypeF2F ||
1739  odom.info().type == (int)Odometry::kTypeViso2 ||
1740  odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size())
1741  {
1742  if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
1743  {
1744  //draw lines
1745  UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
1746  std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
1747  int subImageWidth = 0;
1748  if(data->cameraModels().size()>1 || data->stereoCameraModels().size()>1)
1749  {
1750  subImageWidth = data->cameraModels().size()?data->cameraModels()[0].imageWidth():data->stereoCameraModels()[0].left().imageWidth();
1751  }
1752  for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
1753  {
1754  if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
1755  {
1756  _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
1757  }
1758  if(_ui->imageView_odometry->isLinesShown())
1759  {
1760  // just draw lines in same camera
1761  if(subImageWidth==0 || int(odom.info().refCorners[i].x/subImageWidth) == int(odom.info().newCorners[i].x/subImageWidth))
1762  {
1763  _ui->imageView_odometry->addLine(
1764  odom.info().newCorners[i].x,
1765  odom.info().newCorners[i].y,
1766  odom.info().refCorners[i].x,
1767  odom.info().refCorners[i].y,
1768  inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
1769  }
1770  }
1771  }
1772  }
1773  }
1774  }
1775  if(!data->imageRaw().empty())
1776  {
1777  _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data->imageRaw().cols, (float)data->imageRaw().rows));
1778  }
1779 
1780  _ui->imageView_odometry->update();
1781 
1782  UDEBUG("Time update imageview: %fs", time.ticks());
1783  }
1784 
1785  if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
1786  {
1787  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
1788  }
1789 
1790  //Process info
1791  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
1792  {
1793  double linVar = uMax3(odom.info().reg.covariance.at<double>(0,0), odom.info().reg.covariance.at<double>(1,1)>=9999?0:odom.info().reg.covariance.at<double>(1,1), odom.info().reg.covariance.at<double>(2,2)>=9999?0:odom.info().reg.covariance.at<double>(2,2));
1794  double angVar = uMax3(odom.info().reg.covariance.at<double>(3,3)>=9999?0:odom.info().reg.covariance.at<double>(3,3), odom.info().reg.covariance.at<double>(4,4)>=9999?0:odom.info().reg.covariance.at<double>(4,4), odom.info().reg.covariance.at<double>(5,5));
1795  _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
1796  _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
1797  _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
1798  _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());
1799  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
1800  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
1801  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
1802  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
1803  _ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
1804  _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
1805  _ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures());
1806  _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
1807  _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());
1808  _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)linVar), _preferencesDialog->isCacheSavedInFigures());
1809  _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)linVar, _preferencesDialog->isCacheSavedInFigures());
1810  _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)angVar), _preferencesDialog->isCacheSavedInFigures());
1811  _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)angVar, _preferencesDialog->isCacheSavedInFigures());
1812  _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1813  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1814  if(odom.info().timeParticleFiltering>0.0f)
1815  {
1816  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1817  }
1818  if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
1819  {
1820  _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
1821  _ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
1822  }
1823  _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
1824  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
1825  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
1826  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
1827  if(odom.info().localBundleTime > 0.0f)
1828  {
1829  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
1830  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
1831  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1832  }
1833  _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
1834  _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());
1835 
1836  Transform odomT;
1837  float dist=0.0f, x,y,z, roll,pitch,yaw;
1838  if(!odom.info().transform.isNull())
1839  {
1840  odomT = odom.info().transform;
1841  odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1842  dist = odom.info().transform.getNorm();
1843  _ui->statsToolBox->updateStat("Odometry/T/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
1844  _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
1845  _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
1846  _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
1847  _ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1848  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1849  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1850  }
1851 
1852  if(!odom.info().transformFiltered.isNull())
1853  {
1854  odomT = odom.info().transformFiltered;
1855  odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1856  dist = odom.info().transformFiltered.getNorm();
1857  _ui->statsToolBox->updateStat("Odometry/TF/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
1858  _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
1859  _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
1860  _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
1861  _ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1862  _ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1863  _ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1864  }
1865  if(odom.info().interval > 0)
1866  {
1867  _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
1868  _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1869  _ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
1870  _ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
1871 
1872  if(!odom.info().guess.isNull())
1873  {
1874  odom.info().guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1875  dist = odom.info().guess.getNorm();
1876  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1877  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
1878  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
1879  }
1880  }
1881 
1882  if(!odom.info().transformGroundTruth.isNull())
1883  {
1884  if(!odomT.isNull())
1885  {
1886  rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
1887  _ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
1888  _ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1889  }
1890 
1891  odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1892  dist = odom.info().transformGroundTruth.getNorm();
1893  _ui->statsToolBox->updateStat("Odometry/TG/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist, _preferencesDialog->isCacheSavedInFigures());
1894  _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
1895  _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
1896  _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
1897  _ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1898  _ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1899  _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1900  if(odom.info().interval > 0)
1901  {
1902  _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1903  _ui->statsToolBox->updateStat("Odometry/SpeedG/mph", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
1904  _ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
1905  }
1906  }
1907 
1908  //cumulative pose
1909  if(!odom.pose().isNull())
1910  {
1911  odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1912  _ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
1913  _ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
1914  _ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
1915  _ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1916  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1917  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1918  }
1919  if(!data->groundTruth().isNull())
1920  {
1921  data->groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1922  _ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), x, _preferencesDialog->isCacheSavedInFigures());
1923  _ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), y, _preferencesDialog->isCacheSavedInFigures());
1924  _ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), z, _preferencesDialog->isCacheSavedInFigures());
1925  _ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1926  _ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1927  _ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1928  }
1929 
1930  if(odom.info().distanceTravelled > 0)
1931  {
1932  _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
1933  }
1934 
1935  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), timeTotal.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
1936  UDEBUG("Time updating Stats toolbox: %fs", time.ticks());
1937  }
1938  _processingOdometry = false;
1939 
1940  Q_EMIT(odometryProcessed());
1941 }
1942 
1943 void MainWindow::processStats(const rtabmap::Statistics & stat)
1944 {
1945  _processingStatistics = true;
1946  ULOGGER_DEBUG("");
1947  QElapsedTimer time, totalTime;
1948  time.start();
1949  totalTime.start();
1950  //Affichage des stats et images
1951 
1952  if(_firstStamp == 0.0)
1953  {
1954  _firstStamp = stat.stamp();
1955  }
1956 
1957  int refMapId = -1, loopMapId = -1;
1958  if(stat.getLastSignatureData().id() == stat.refImageId())
1959  {
1960  refMapId = stat.getLastSignatureData().mapId();
1961  }
1962  int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
1963  int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
1964  if(loopId>0 && _cachedSignatures.contains(loopId))
1965  {
1966  loopMapId = _cachedSignatures.value(loopId).mapId();
1967  }
1968 
1969  _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
1970 
1971  if(stat.extended())
1972  {
1973  float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
1974  if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
1975  {
1976  UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
1977  }
1978 
1979  UDEBUG("");
1980  bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
1981 
1982  bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
1983 
1984  bool fastMovement = (bool)uValue(stat.data(), Statistics::kMemoryFast_movement(), 0.0f);
1985 
1986  int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1987 
1988  // update cache
1989  Signature signature;
1990  if(stat.getLastSignatureData().id() == stat.refImageId())
1991  {
1992  signature = stat.getLastSignatureData();
1993  }
1994  else if(rehearsalMerged>0 &&
1995  rehearsalMerged == stat.getLastSignatureData().id() &&
1996  _cachedSignatures.contains(rehearsalMerged))
1997  {
1998  signature = _cachedSignatures.value(rehearsalMerged);
1999  }
2000 
2001  if(signature.id()!=0)
2002  {
2003  // make sure data are uncompressed
2004  // We don't need to uncompress images if we don't show them
2005  bool uncompressImages = !signature.sensorData().imageCompressed().empty() && (
2006  _ui->imageView_source->isVisible() ||
2007  (_loopClosureViewer->isVisible() &&
2008  !signature.sensorData().depthOrRightCompressed().empty()) ||
2009  (_cloudViewer->isVisible() &&
2010  _preferencesDialog->isCloudsShown(0) &&
2011  !signature.sensorData().depthOrRightCompressed().empty()));
2012  bool uncompressScan = !signature.sensorData().laserScanCompressed().isEmpty() && (
2013  _loopClosureViewer->isVisible() ||
2014  (_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0)));
2015  cv::Mat tmpRgb, tmpDepth, tmpG, tmpO, tmpE;
2016  LaserScan tmpScan;
2017  signature.sensorData().uncompressData(
2018  uncompressImages?&tmpRgb:0,
2019  uncompressImages && !signature.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0,
2020  uncompressScan?&tmpScan:0,
2021  0, &tmpG, &tmpO, &tmpE);
2022 
2023  if( stat.getLastSignatureData().id() == stat.refImageId() &&
2024  uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
2025  signature.getWeight()>=0) // ignore intermediate nodes for the cache
2026  {
2027  if(smallMovement || fastMovement)
2028  {
2029  _cachedSignatures.insert(0, signature); // zero means temporary
2030  }
2031  else
2032  {
2033  _cachedSignatures.insert(signature.id(), signature);
2034  _cachedMemoryUsage += signature.sensorData().getMemoryUsed();
2035  unsigned int count = 0;
2036  if(!signature.getWords3().empty())
2037  {
2038  for(std::multimap<int, int>::const_iterator jter=signature.getWords().upper_bound(-1); jter!=signature.getWords().end(); ++jter)
2039  {
2040  if(util3d::isFinite(signature.getWords3()[jter->second]))
2041  {
2042  ++count;
2043  }
2044  }
2045  }
2046  _cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
2047  }
2048  }
2049  }
2050 
2051  // Add data
2052  for(std::map<int, Signature>::const_iterator iter = stat.getSignaturesData().begin();
2053  iter!=stat.getSignaturesData().end();
2054  ++iter)
2055  {
2056  if(signature.id() != iter->first &&
2057  (!_cachedSignatures.contains(iter->first) ||
2058  (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty())))
2059  {
2060  _cachedSignatures.insert(iter->first, iter->second);
2061  _cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
2062  unsigned int count = 0;
2063  if(!iter->second.getWords3().empty())
2064  {
2065  for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
2066  {
2067  if(util3d::isFinite(iter->second.getWords3()[jter->second]))
2068  {
2069  ++count;
2070  }
2071  }
2072  }
2073  _cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
2074  UINFO("Added node data %d [map=%d] to cache", iter->first, iter->second.mapId());
2075 
2076  _currentMapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
2077  if(!iter->second.getGroundTruthPose().isNull())
2078  {
2079  _currentGTPosesMap.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
2080  }
2081  }
2082  }
2083 
2084  // For intermediate empty nodes, keep latest image shown
2085  if(signature.getWeight() >= 0)
2086  {
2087  _ui->imageView_source->clear();
2088  _ui->imageView_loopClosure->clear();
2089 
2090  if(signature.sensorData().imageRaw().empty() && signature.getWords().empty())
2091  {
2092  // To see colors
2093  _ui->imageView_source->setSceneRect(QRect(0,0,640,480));
2094  }
2095 
2096  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
2097  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
2098 
2099  _ui->label_matchId->clear();
2100 
2101  bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
2102  int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
2103  bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
2104  _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
2105 
2106  if(rehearsalMerged > 0)
2107  {
2108  _ui->imageView_source->setBackgroundColor(Qt::blue);
2109  }
2110  else if(proximityTimeDetections > 0)
2111  {
2112  _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
2113  }
2114  else if(scanMatchingSuccess)
2115  {
2116  _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
2117  }
2118  else if(rehearsedSimilarity)
2119  {
2120  _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
2121  }
2122  else if(smallMovement)
2123  {
2124  _ui->imageView_source->setBackgroundColor(Qt::gray);
2125  }
2126  else if(fastMovement)
2127  {
2128  _ui->imageView_source->setBackgroundColor(Qt::magenta);
2129  }
2130  // Set color code as tooltip
2131  if(_ui->label_refId->toolTip().isEmpty())
2132  {
2133  _ui->label_refId->setToolTip(
2134  "Background Color Code:\n"
2135  " Blue = Weight Update Merged\n"
2136  " Dark Blue = Weight Update\n"
2137  " Dark Yellow = Proximity Detection in Time\n"
2138  " Dark Cyan = Neighbor Link Refined\n"
2139  " Gray = Small Movement\n"
2140  " Magenta = Fast Movement\n"
2141  "Feature Color code:\n"
2142  " Green = New\n"
2143  " Yellow = New but Not Unique\n"
2144  " Red = In Vocabulary\n"
2145  " Blue = In Vocabulary and in Previous Signature\n"
2146  " Pink = In Vocabulary and in Loop Closure Signature\n"
2147  " Gray = Not Quantized to Vocabulary");
2148  }
2149  // Set color code as tooltip
2150  if(_ui->label_matchId->toolTip().isEmpty())
2151  {
2152  _ui->label_matchId->setToolTip(
2153  "Background Color Code:\n"
2154  " Green = Accepted Loop Closure Detection\n"
2155  " Red = Rejected Loop Closure Detection\n"
2156  " Yellow = Proximity Detection in Space\n"
2157  "Feature Color code:\n"
2158  " Red = In Vocabulary\n"
2159  " Pink = In Vocabulary and in Loop Closure Signature\n"
2160  " Gray = Not Quantized to Vocabulary");
2161  }
2162 
2163  int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
2164  float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
2165  int landmarkId = static_cast<int>(uValue(stat.data(), Statistics::kLoopLandmark_detected(), 0.0f));
2166  int landmarkNodeRef = static_cast<int>(uValue(stat.data(), Statistics::kLoopLandmark_detected_node_ref(), 0.0f));
2167  int matchId = 0;
2168  Signature loopSignature;
2169  int shownLoopId = 0;
2170  if(highestHypothesisId > 0 || stat.proximityDetectionId()>0 || landmarkId>0)
2171  {
2172  bool show = true;
2173  if(stat.loopClosureId() > 0)
2174  {
2175  _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
2176  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
2177  if(highestHypothesisIsSaved)
2178  {
2179  _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
2180  }
2181  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
2182  matchId = stat.loopClosureId();
2183  }
2184  else if(stat.proximityDetectionId())
2185  {
2186  _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
2187  _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
2188  matchId = stat.proximityDetectionId();
2189  }
2190  else if(landmarkId!=0)
2191  {
2192  highestHypothesisId = landmarkNodeRef;
2193  if(rejectedHyp)
2194  {
2195  show = _preferencesDialog->imageRejectedShown();
2196  if(show)
2197  {
2198  _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
2199  _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
2200  _ui->label_matchId->setText(QString("Landmark rejected = %1 with %2").arg(landmarkId).arg(landmarkNodeRef));
2201  }
2202  }
2203  else
2204  {
2205  _ui->imageView_loopClosure->setBackgroundColor(QColor("orange"));
2206  _ui->label_matchId->setText(QString("Landmark match = %1 with %2").arg(landmarkId).arg(landmarkNodeRef));
2207  matchId = landmarkNodeRef;
2208  }
2209  }
2210  else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
2211  {
2212  show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
2213  if(show)
2214  {
2215  _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
2216  _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
2217  _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
2218  }
2219  }
2220  else
2221  {
2222  show = _preferencesDialog->imageHighestHypShown();
2223  if(show)
2224  {
2225  _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
2226  }
2227  }
2228 
2229  if(show)
2230  {
2231  shownLoopId = matchId>0?matchId:highestHypothesisId;
2232  QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
2233  if(iter != _cachedSignatures.end())
2234  {
2235  // uncompress after copy to avoid keeping uncompressed data in memory
2236  loopSignature = iter.value();
2237  bool uncompressImages = !loopSignature.sensorData().imageCompressed().empty() && (
2238  _ui->imageView_source->isVisible() ||
2239  (_loopClosureViewer->isVisible() &&
2240  !loopSignature.sensorData().depthOrRightCompressed().empty()));
2241  bool uncompressScan = _loopClosureViewer->isVisible() &&
2242  !loopSignature.sensorData().laserScanCompressed().isEmpty();
2243  if(uncompressImages || uncompressScan)
2244  {
2245  cv::Mat tmpRGB, tmpDepth;
2246  LaserScan tmpScan;
2247  loopSignature.sensorData().uncompressData(
2248  uncompressImages?&tmpRGB:0,
2249  uncompressImages?&tmpDepth:0,
2250  uncompressScan?&tmpScan:0);
2251  }
2252  }
2253  }
2254  }
2255  _refIds.push_back(stat.refImageId());
2256  _loopClosureIds.push_back(matchId);
2257  if(matchId > 0)
2258  {
2259  _cachedLocalizationsCount[matchId] += 1.0f;
2260  }
2261  UDEBUG("time= %d ms (update detection ui)", time.restart());
2262 
2263  //update image views
2264  if(!signature.sensorData().imageRaw().empty() ||
2265  !loopSignature.sensorData().imageRaw().empty() ||
2266  signature.getWords().size())
2267  {
2268  cv::Mat refImage = signature.sensorData().imageRaw();
2269  cv::Mat loopImage = loopSignature.sensorData().imageRaw();
2270 
2271  if( _preferencesDialog->isMarkerDetection() &&
2272  _preferencesDialog->isLandmarksShown())
2273  {
2274  //draw markers
2275  if(!signature.getLandmarks().empty())
2276  {
2277  if(refImage.channels() == 1)
2278  {
2279  cv::Mat imgColor;
2280  cvtColor(refImage, imgColor, cv::COLOR_GRAY2BGR);
2281  refImage = imgColor;
2282  }
2283  else
2284  {
2285  refImage = refImage.clone();
2286  }
2287  drawLandmarks(refImage, signature);
2288  }
2289  if(!loopSignature.getLandmarks().empty())
2290  {
2291  if(loopImage.channels() == 1)
2292  {
2293  cv::Mat imgColor;
2294  cv::cvtColor(loopImage, imgColor, cv::COLOR_GRAY2BGR);
2295  loopImage = imgColor;
2296  }
2297  else
2298  {
2299  loopImage = loopImage.clone();
2300  }
2301  drawLandmarks(loopImage, loopSignature);
2302  }
2303  }
2304 
2305  UCvMat2QImageThread qimageThread(refImage);
2306  UCvMat2QImageThread qimageLoopThread(loopImage);
2307  qimageThread.start();
2308  qimageLoopThread.start();
2309  qimageThread.join();
2310  qimageLoopThread.join();
2311  QImage img = qimageThread.getQImage();
2312  QImage lcImg = qimageLoopThread.getQImage();
2313  UDEBUG("time= %d ms (convert image to qt)", time.restart());
2314 
2315  if(!img.isNull())
2316  {
2317  _ui->imageView_source->setImage(img);
2318  }
2319  if(!signature.sensorData().depthOrRightRaw().empty())
2320  {
2321  _ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
2322  }
2323  if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
2324  {
2325  QRect sceneRect;
2326  if(signature.sensorData().cameraModels().size())
2327  {
2328  for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
2329  {
2330  sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
2331  sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
2332  }
2333  }
2334  else if(signature.sensorData().stereoCameraModels().size())
2335  {
2336  for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
2337  {
2338  sceneRect.setWidth(sceneRect.width()+signature.sensorData().stereoCameraModels()[i].left().imageWidth());
2339  sceneRect.setHeight(sceneRect.height()+signature.sensorData().stereoCameraModels()[i].left().imageHeight());
2340  }
2341  }
2342  if(sceneRect.isValid())
2343  {
2344  _ui->imageView_source->setSceneRect(sceneRect);
2345  }
2346  }
2347  if(!lcImg.isNull())
2348  {
2349  _ui->imageView_loopClosure->setImage(lcImg);
2350  }
2351  if(!loopSignature.sensorData().depthOrRightRaw().empty())
2352  {
2353  _ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
2354  }
2355  if(_ui->imageView_loopClosure->sceneRect().isNull())
2356  {
2357  _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
2358  }
2359  }
2360  else if(_ui->imageView_loopClosure->sceneRect().isNull() &&
2361  !_ui->imageView_source->sceneRect().isNull())
2362  {
2363  _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
2364  }
2365 
2366  UDEBUG("time= %d ms (update detection imageviews)", time.restart());
2367 
2368  // do it after scaling
2369  std::multimap<int, cv::KeyPoint> wordsA;
2370  std::multimap<int, cv::KeyPoint> wordsB;
2371  for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
2372  {
2373  wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
2374  }
2375  for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
2376  {
2377  wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
2378  }
2379  this->drawKeypoints(wordsA, wordsB);
2380 
2381  UDEBUG("time= %d ms (draw keypoints)", time.restart());
2382 
2383  // loop closure view
2384  if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
2385  !stat.loopClosureTransform().isNull() &&
2386  !loopSignature.sensorData().imageRaw().empty())
2387  {
2388  // the last loop closure data
2389  Transform loopClosureTransform = stat.loopClosureTransform();
2390  signature.setPose(loopClosureTransform);
2391  _loopClosureViewer->setData(loopSignature, signature);
2392  if(_ui->dockWidget_loopClosureViewer->isVisible())
2393  {
2394  UTimer loopTimer;
2395  _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
2396  UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
2397  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
2398  {
2399  _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
2400  }
2401  }
2402 
2403  UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
2404  }
2405  }
2406 
2407  // PDF AND LIKELIHOOD
2408  if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
2409  {
2410  UDEBUG("");
2411  _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
2412 
2413  ULOGGER_DEBUG("");
2414  //Adjust thresholds
2415  Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
2416  }
2417  if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
2418  {
2419  _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
2420  }
2421  if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
2422  {
2423  _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
2424  }
2425  UDEBUG("time= %d ms (update likelihood and posterior)", time.restart());
2426 
2427  // Update statistics tool box
2428  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
2429  {
2430  const std::map<std::string, float> & statistics = stat.data();
2431  std::string odomStr = "Odometry/";
2432  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
2433  {
2434  //ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
2435  if((*iter).first.size()<odomStr.size() || (*iter).first.substr(0, odomStr.size()).compare(odomStr)!=0)
2436  {
2437  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
2438  }
2439  }
2440  }
2441 
2442  UDEBUG("time= %d ms (update stats toolbox)", time.restart());
2443 
2444  //======================
2445  // RGB-D Mapping stuff
2446  //======================
2447  if(!stat.mapCorrection().isNull())
2448  {
2449  _odometryCorrection = stat.mapCorrection();
2450  }
2451  // update clouds
2452  if(stat.poses().size())
2453  {
2454  // update pose only if odometry is not received
2455  std::map<int, int> mapIds = _currentMapIds;
2456  std::map<int, Transform> groundTruth = _currentGTPosesMap;
2457 
2458  mapIds.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().mapId()));
2459  if(!stat.getLastSignatureData().getGroundTruthPose().isNull() &&
2460  _cachedSignatures.contains(stat.getLastSignatureData().id()))
2461  {
2462  groundTruth.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getGroundTruthPose()));
2463  }
2464 
2465  if(_preferencesDialog->isPriorIgnored() &&
2466  _ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
2467  stat.getLastSignatureData().sensorData().gps().stamp()!=0.0 &&
2468  stat.poses().find(stat.getLastSignatureData().id())!=stat.poses().end())
2469  {
2470  float bearing = (float)((-(stat.getLastSignatureData().sensorData().gps().bearing()-90))*M_PI/180.0);
2471  float gpsRotationOffset = stat.poses().at(stat.getLastSignatureData().id()).theta()-bearing;
2472  _ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset);
2473  }
2474  else if(!_preferencesDialog->isPriorIgnored() &&
2475  _ui->graphicsView_graphView->getWorldMapRotation() != 0.0f)
2476  {
2477  _ui->graphicsView_graphView->setWorldMapRotation(0.0f);
2478  }
2479 
2480 
2481  std::map<int, Transform> poses = stat.poses();
2482 
2483  UDEBUG("time= %d ms (update gt-gps stuff)", time.restart());
2484 
2485  UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId());
2486  if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
2487  {
2488  if(poses.rbegin()->first == stat.getLastSignatureData().id())
2489  {
2490  if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
2491  {
2492  _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
2493  }
2494  else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection())
2495  {
2496  _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels());
2497  }
2498  else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() ||
2499  !stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty())
2500  {
2501  Transform scanLocalTransform;
2502  if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty())
2503  {
2504  scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform();
2505  }
2506  else
2507  {
2508  scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform();
2509  }
2510  //fake frustum
2511  CameraModel model(
2512  2,
2513  2,
2514  2,
2515  1.5,
2516  scanLocalTransform*CameraModel::opticalRotation(),
2517  0,
2518  cv::Size(4,3));
2519  _cloudViewer->updateCameraFrustum(poses.rbegin()->second, model);
2520  }
2521  }
2522 
2523  _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
2524 
2525  if(_ui->graphicsView_graphView->isVisible())
2526  {
2527  _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
2528  }
2529  }
2530 
2531  if(_cachedSignatures.contains(0) && stat.refImageId()>0)
2532  {
2533  if(poses.find(stat.refImageId())!=poses.end())
2534  {
2535  poses.insert(std::make_pair(0, poses.at(stat.refImageId())));
2536  poses.erase(stat.refImageId());
2537  }
2538  if(groundTruth.find(stat.refImageId())!=groundTruth.end())
2539  {
2540  groundTruth.insert(std::make_pair(0, groundTruth.at(stat.refImageId())));
2541  groundTruth.erase(stat.refImageId());
2542  }
2543  }
2544 
2545  std::map<std::string, float> updateCloudSats;
2546  updateMapCloud(
2547  poses,
2548  stat.constraints(),
2549  mapIds,
2550  stat.labels(),
2551  groundTruth,
2552  stat.odomCachePoses(),
2553  stat.odomCacheConstraints(),
2554  false,
2555  &updateCloudSats);
2556 
2557  _odometryReceived = false;
2558 
2559  UDEBUG("time= %d ms (update map cloud)", time.restart());
2560 
2561  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
2562  {
2563  for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
2564  {
2565  _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
2566  }
2567  }
2568 
2569 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2570  if(_preferencesDialog->isFramesShown())
2571  {
2572  _cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
2573  _cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
2574  _cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), true, false);
2575  if(_preferencesDialog->isLabelsShown())
2576  {
2577  _cloudViewer->addOrUpdateText("map_frame_label", "map", Transform::getIdentity(), 0.1, Qt::white);
2578  _cloudViewer->addOrUpdateText("odom_frame_label", "odom", _odometryCorrection, 0.1, Qt::white);
2579  }
2580  }
2581  else
2582  {
2583  _cloudViewer->removeLine("map_to_odom");
2584  _cloudViewer->removeCoordinate("odom_frame");
2585  _cloudViewer->removeCoordinate("map_frame");
2586  _cloudViewer->removeText("map_frame_label");
2587  _cloudViewer->removeText("odom_frame_label");
2588  }
2589 #endif
2590  }
2591 
2592  if( _ui->graphicsView_graphView->isVisible())
2593  {
2594  // update posterior on the graph view
2595  if(_preferencesDialog->isPosteriorGraphView() &&
2596  stat.posterior().size())
2597  {
2598  _ui->graphicsView_graphView->updatePosterior(stat.posterior());
2599  }
2600  else if(_preferencesDialog->isRGBDMode())
2601  {
2602  if(_preferencesDialog->isWordsCountGraphView() &&
2603  _cachedWordsCount.size())
2604  {
2605  _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
2606  }
2607  else if(_preferencesDialog->isLocalizationsCountGraphView() &&
2608  _cachedLocalizationsCount.size())
2609  {
2610  _ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
2611  }
2612  }
2613  if(_preferencesDialog->isRelocalizationColorOdomCacheGraphView() && !stat.odomCachePoses().empty())
2614  {
2615  std::map<int, float> colors;
2616  colors.insert(std::make_pair(stat.odomCachePoses().rbegin()->first, 240));
2617  for(std::multimap<int, Link>::const_iterator iter=stat.odomCacheConstraints().begin(); iter!=stat.odomCacheConstraints().end(); ++iter)
2618  {
2619  if(iter->second.type() != Link::kNeighbor)
2620  {
2621  uInsert(colors, std::pair<int,float>(iter->second.from()>iter->second.to()?iter->second.from():iter->second.to(), 120)); //green
2622  }
2623  }
2624  for(std::map<int, Transform>::const_iterator iter=stat.odomCachePoses().begin(); iter!=stat.odomCachePoses().end(); ++iter)
2625  {
2626  if(stat.poses().find(iter->first) == stat.poses().end())
2627  {
2628  colors.insert(std::make_pair(iter->first, 240)); //red
2629  }
2630  }
2631  _ui->graphicsView_graphView->updatePosterior(colors, 240);
2632  }
2633  // update local path on the graph view
2634  _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
2635  if(stat.localPath().size() == 0)
2636  {
2637  // clear the global path if set (goal reached)
2638  _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
2639  }
2640  // update current goal id
2641  if(stat.currentGoalId() > 0)
2642  {
2643  _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
2644  }
2645  UDEBUG("time= %d ms (update graph view)", time.restart());
2646  }
2647 
2648  if(_multiSessionLocWidget->isVisible())
2649  {
2650  _multiSessionLocWidget->updateView(signature, stat);
2651  }
2652 
2653  _cachedSignatures.remove(0); // remove tmp negative ids
2654 
2655  // keep only compressed data in cache
2656  if(_cachedSignatures.contains(stat.refImageId()))
2657  {
2658  Signature & s = *_cachedSignatures.find(stat.refImageId());
2659  _cachedMemoryUsage -= s.sensorData().getMemoryUsed();
2660  s.sensorData().clearRawData();
2661  s.sensorData().clearOccupancyGridRaw();
2662  _cachedMemoryUsage += s.sensorData().getMemoryUsed();
2663  }
2664 
2665  // Check missing cache
2666  if(stat.getSignaturesData().size() <= 1)
2667  {
2668  if(_preferencesDialog->isMissingCacheRepublished() &&
2669  _preferencesDialog->isImagesKept() &&
2670  atoi(_preferencesDialog->getParameter(Parameters::kRtabmapMaxRepublished()).c_str()) > 0)
2671  {
2672  std::vector<int> missingIds;
2673  bool ignoreNewData = smallMovement || fastMovement || signature.getWeight()<0;
2674  std::set<int> ids = uKeysSet(stat.poses());
2675  if(ids.empty())
2676  {
2677  // In appearance-only mode
2678  ids = uKeysSet(stat.posterior());
2679  }
2680  for(std::set<int>::const_iterator iter=ids.lower_bound(1); iter!=ids.end(); ++iter)
2681  {
2682  if(!ignoreNewData || stat.refImageId() != *iter)
2683  {
2684  QMap<int, Signature>::iterator ster = _cachedSignatures.find(*iter);
2685  if(ster == _cachedSignatures.end() ||
2686  (ster.value().getWeight() >=0 && // ignore intermediate nodes
2687  ster.value().sensorData().imageCompressed().empty() &&
2688  ster.value().sensorData().depthOrRightCompressed().empty() &&
2689  ster.value().sensorData().laserScanCompressed().empty()))
2690  {
2691  missingIds.push_back(*iter);
2692  }
2693  }
2694  }
2695  if(!missingIds.empty())
2696  {
2697  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRepublishData, UVariant(missingIds)));
2698  }
2699  }
2700  }
2701 
2702  UDEBUG("time= %d ms (update cache)", time.restart());
2703  }
2704  else if(!stat.extended() && stat.loopClosureId()>0)
2705  {
2706  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
2707  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
2708  }
2709  else
2710  {
2711  _ui->label_matchId->clear();
2712  }
2713  float elapsedTime = static_cast<float>(totalTime.elapsed());
2714  UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
2715  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
2716  {
2717  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
2718  }
2719  if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
2720  {
2721  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
2722  }
2723 
2724  if(!_preferencesDialog->isImagesKept())
2725  {
2726  _cachedSignatures.clear();
2727  _cachedMemoryUsage = 0;
2728  _cachedWordsCount.clear();
2729  }
2730  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
2731  {
2732  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2733  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2734 #ifdef RTABMAP_OCTOMAP
2735  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2736 #endif
2737  }
2738  if(_state != kMonitoring && _state != kDetecting)
2739  {
2740  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2741  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2742  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2743  }
2744 
2745  _processingStatistics = false;
2746 
2747  Q_EMIT(statsProcessed());
2748 }
2749 
2750 void MainWindow::updateMapCloud(
2751  const std::map<int, Transform> & posesIn,
2752  const std::multimap<int, Link> & constraints,
2753  const std::map<int, int> & mapIdsIn,
2754  const std::map<int, std::string> & labels,
2755  const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
2756  const std::map<int, Transform> & odomCachePoses,
2757  const std::multimap<int, Link> & odomCacheConstraints,
2758  bool verboseProgress,
2759  std::map<std::string, float> * stats)
2760 {
2761  UTimer timer;
2762  std::map<int, Transform> nodePoses(posesIn.lower_bound(0), posesIn.end());
2763  UDEBUG("nodes=%d landmarks=%d constraints=%d mapIdsIn=%d labelsIn=%d",
2764  (int)nodePoses.size(), (int)(posesIn.size() - nodePoses.size()), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
2765  if(posesIn.size())
2766  {
2767  _currentPosesMap = posesIn;
2768  _currentPosesMap.erase(0); // don't keep 0 if it is there
2769  _currentLinksMap = constraints;
2770  _currentMapIds = mapIdsIn;
2771  _currentLabels = labels;
2772  _currentGTPosesMap = groundTruths;
2773  _currentGTPosesMap.erase(0);
2774  if(_state != kMonitoring && _state != kDetecting)
2775  {
2776  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && nodePoses.size() >= 2 && _currentLinksMap.size() >= 1);
2777  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
2778  }
2779  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
2780  }
2781 
2782  // filter duplicated poses
2783  std::map<int, Transform> poses;
2784  std::map<int, int> mapIds;
2785  if(_preferencesDialog->isCloudFiltering() && nodePoses.size())
2786  {
2787  float radius = _preferencesDialog->getCloudFilteringRadius();
2788  float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
2789  bool hasZero = nodePoses.find(0) != nodePoses.end();
2790  if(hasZero)
2791  {
2792  std::map<int, Transform> posesInTmp = nodePoses;
2793  posesInTmp.erase(0);
2794  poses = rtabmap::graph::radiusPosesFiltering(posesInTmp, radius, angle);
2795  }
2796  else
2797  {
2798  poses = rtabmap::graph::radiusPosesFiltering(nodePoses, radius, angle);
2799  }
2800  for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
2801  {
2802  std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
2803  if(jter!=mapIdsIn.end())
2804  {
2805  mapIds.insert(*jter);
2806  }
2807  }
2808  //keep 0
2809  if(hasZero)
2810  {
2811  poses.insert(*nodePoses.find(0));
2812  }
2813 
2814  if(verboseProgress)
2815  {
2816  _progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(nodePoses.size()));
2817  QApplication::processEvents();
2818  }
2819  }
2820  else
2821  {
2822  poses = nodePoses;
2823  mapIds = mapIdsIn;
2824  }
2825 
2826  std::map<int, bool> posesMask;
2827  for(std::map<int, Transform>::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter)
2828  {
2829  posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
2830  }
2831  _ui->widget_mapVisibility->setMap(nodePoses, posesMask);
2832 
2833  if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
2834  {
2835  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2836  {
2837  std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
2838  if(gtIter!=groundTruths.end())
2839  {
2840  iter->second = gtIter->second;
2841  }
2842  else
2843  {
2844  UWARN("Not found ground truth pose for node %d", iter->first);
2845  }
2846  }
2847  }
2848  else if(_currentGTPosesMap.size() == 0)
2849  {
2850  _ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
2851  }
2852 
2853  int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
2854  int altitudeDelta = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalAltitudeDelta()));
2855  if((maxNodes > 0 || altitudeDelta>0.0) && poses.size()>1)
2856  {
2857  Transform currentPose = poses.rbegin()->second;
2858  if(poses.find(0) != poses.end())
2859  {
2860  currentPose = poses.at(0);
2861  }
2862 
2863  std::map<int, Transform> nearestPoses;
2864  if(maxNodes > 0)
2865  {
2866  std::map<int, float> nodes = graph::findNearestNodes(currentPose, poses, 0, 0, maxNodes);
2867  for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2868  {
2869  if(altitudeDelta<=0.0 ||
2870  fabs(poses.at(iter->first).z()-currentPose.z())<altitudeDelta)
2871  {
2872  nearestPoses.insert(*poses.find(iter->first));
2873  }
2874  }
2875  }
2876  else // altitudeDelta>0.0
2877  {
2878  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2879  {
2880  if(fabs(iter->second.z()-currentPose.z())<altitudeDelta)
2881  {
2882  nearestPoses.insert(*iter);
2883  }
2884  }
2885  }
2886 
2887  //add zero...
2888  if(poses.find(0) != poses.end())
2889  {
2890  nearestPoses.insert(*poses.find(0));
2891  }
2892  poses=nearestPoses;
2893  }
2894 
2895  // Map updated! regenerate the assembled cloud, last pose is the new one
2896  UDEBUG("Update map with %d locations", poses.size());
2897  QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
2898  std::set<std::string> viewerLines = _cloudViewer->getAddedLines();
2899  int i=1;
2900  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2901  {
2902  if(!iter->second.isNull())
2903  {
2904  std::string cloudName = uFormat("cloud%d", iter->first);
2905 
2906  if(iter->first == 0)
2907  {
2908  viewerClouds.remove(cloudName);
2909  _cloudViewer->removeCloud(cloudName);
2910  }
2911 
2912  // 3d point cloud
2913  bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
2914  if(update3dCloud)
2915  {
2916  // update cloud
2917  if(viewerClouds.contains(cloudName))
2918  {
2919  // Update only if the pose has changed
2920  Transform tCloud;
2921  _cloudViewer->getPose(cloudName, tCloud);
2922  if(tCloud.isNull() || iter->second != tCloud)
2923  {
2924  if(!_cloudViewer->updateCloudPose(cloudName, iter->second))
2925  {
2926  UERROR("Updating pose cloud %d failed!", iter->first);
2927  }
2928  }
2929  _cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
2930  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
2931  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
2932  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
2933  }
2934  else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
2935  _cachedClouds.find(iter->first) == _cachedClouds.end() &&
2936  _cachedSignatures.contains(iter->first))
2937  {
2938  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2939  if(_cloudViewer->getAddedClouds().contains(cloudName))
2940  {
2941  _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
2942  }
2943  }
2944  }
2945  else if(viewerClouds.contains(cloudName))
2946  {
2947  _cloudViewer->setCloudVisibility(cloudName.c_str(), false);
2948  }
2949 
2950  // 2d point cloud
2951  std::string scanName = uFormat("scan%d", iter->first);
2952  if(iter->first == 0)
2953  {
2954  viewerClouds.remove(scanName);
2955  _cloudViewer->removeCloud(scanName);
2956  }
2957  if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
2958  {
2959  if(viewerClouds.contains(scanName))
2960  {
2961  // Update only if the pose has changed
2962  Transform tScan;
2963  _cloudViewer->getPose(scanName, tScan);
2964  if(tScan.isNull() || iter->second != tScan)
2965  {
2966  if(!_cloudViewer->updateCloudPose(scanName, iter->second))
2967  {
2968  UERROR("Updating pose scan %d failed!", iter->first);
2969  }
2970  }
2971  _cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
2972  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
2973  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
2974  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
2975  }
2976  else if(_cachedSignatures.contains(iter->first))
2977  {
2978  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2979  if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
2980  {
2981  this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2982  }
2983  }
2984  }
2985  else if(viewerClouds.contains(scanName))
2986  {
2987  _cloudViewer->setCloudVisibility(scanName.c_str(), false);
2988  }
2989 
2990  // occupancy grids
2991  bool updateGridMap =
2992  ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
2993  (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
2994  _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
2995  bool updateOctomap = false;
2996  bool updateElevationMap = false;
2997 #ifdef RTABMAP_OCTOMAP
2998  updateOctomap =
2999  _cloudViewer->isVisible() &&
3000  _preferencesDialog->isOctomapUpdated() &&
3001  _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
3002 #endif
3003 #ifdef RTABMAP_GRIDMAP
3004  updateElevationMap =
3005  _cloudViewer->isVisible() &&
3006  _preferencesDialog->getElevationMapShown() > 0 &&
3007  _elevationMap->addedNodes().find(iter->first) == _elevationMap->addedNodes().end();
3008 #endif
3009  if(updateGridMap || updateOctomap || updateElevationMap)
3010  {
3011  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
3012  if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
3013  {
3014  cv::Mat ground;
3015  cv::Mat obstacles;
3016  cv::Mat empty;
3017 
3018  jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
3019 
3020  double resolution = jter->sensorData().gridCellSize();
3021  if(_preferencesDialog->getGridUIResolution() > jter->sensorData().gridCellSize())
3022  {
3023  resolution = _preferencesDialog->getGridUIResolution();
3024  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(ground)), resolution);
3025  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(obstacles)), resolution);
3026  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(empty)), resolution);
3027  if(ground.channels() == 2)
3028  ground = util3d::laserScan2dFromPointCloud(*groundCloud).data();
3029  else
3030  ground = util3d::laserScanFromPointCloud(*groundCloud).data();
3031  if(obstacles.channels() == 2)
3032  obstacles = util3d::laserScan2dFromPointCloud(*obstaclesCloud).data();
3033  else
3034  obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud).data();
3035  if(empty.channels() == 2)
3036  empty = util3d::laserScan2dFromPointCloud(*emptyCloud).data();
3037  else
3038  empty = util3d::laserScanFromPointCloud(*emptyCloud).data();
3039  }
3040 
3041  _cachedLocalMaps.add(iter->first, ground, obstacles, empty, resolution, jter->sensorData().gridViewPoint());
3042  }
3043  }
3044 
3045  // 3d features
3046  std::string featuresName = uFormat("features%d", iter->first);
3047  if(iter->first == 0)
3048  {
3049  viewerClouds.remove(featuresName);
3050  _cloudViewer->removeCloud(featuresName);
3051  }
3052  if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
3053  {
3054  if(viewerClouds.contains(featuresName))
3055  {
3056  // Update only if the pose has changed
3057  Transform tFeatures;
3058  _cloudViewer->getPose(featuresName, tFeatures);
3059  if(tFeatures.isNull() || iter->second != tFeatures)
3060  {
3061  if(!_cloudViewer->updateCloudPose(featuresName, iter->second))
3062  {
3063  UERROR("Updating pose features %d failed!", iter->first);
3064  }
3065  }
3066  _cloudViewer->setCloudVisibility(featuresName, _preferencesDialog->isFeaturesShown(0));
3067  _cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
3068  }
3069  else if(_cachedSignatures.contains(iter->first))
3070  {
3071  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
3072  if(!jter->getWords3().empty())
3073  {
3074  this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
3075  }
3076  }
3077  }
3078  else if(viewerClouds.contains(featuresName))
3079  {
3080  _cloudViewer->setCloudVisibility(featuresName.c_str(), false);
3081  }
3082 
3083  // Gravity arrows
3084  std::string gravityName = uFormat("gravity%d", iter->first);
3085  if(iter->first == 0)
3086  {
3087  viewerLines.erase(gravityName);
3088  _cloudViewer->removeLine(gravityName);
3089  }
3090  if(_cloudViewer->isVisible() && _preferencesDialog->isIMUGravityShown(0))
3091  {
3092  std::multimap<int, Link>::const_iterator linkIter = graph::findLink(constraints, iter->first, iter->first, false, Link::kGravity);
3093  if(linkIter != constraints.end())
3094  {
3095  Transform gravityT = linkIter->second.transform();
3096  Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0));
3097  gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity;
3098  _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);
3099  }
3100  }
3101  else if(viewerLines.find(gravityName)!=viewerLines.end())
3102  {
3103  _cloudViewer->removeLine(gravityName.c_str());
3104  }
3105 
3106  if(verboseProgress)
3107  {
3108  _progressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
3109  _progressDialog->incrementStep();
3110  if(poses.size() < 200 || i % 100 == 0)
3111  {
3112  QApplication::processEvents();
3113  if(_progressCanceled)
3114  {
3115  break;
3116  }
3117  }
3118  }
3119  }
3120 
3121  ++i;
3122  }
3123 
3124  //remove not used clouds
3125  for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
3126  {
3127  std::list<std::string> splitted = uSplitNumChar(iter.key());
3128  int id = 0;
3129  if(splitted.size() == 2)
3130  {
3131  id = std::atoi(splitted.back().c_str());
3132  if(poses.find(id) == poses.end())
3133  {
3134  if(_cloudViewer->getCloudVisibility(iter.key()))
3135  {
3136  UDEBUG("Hide %s", iter.key().c_str());
3137  _cloudViewer->setCloudVisibility(iter.key(), false);
3138  }
3139  }
3140  }
3141  }
3142  // remove not used gravity lines
3143  for(std::set<std::string>::iterator iter = viewerLines.begin(); iter!=viewerLines.end(); ++iter)
3144  {
3145  std::list<std::string> splitted = uSplitNumChar(*iter);
3146  int id = 0;
3147  if(splitted.size() == 2)
3148  {
3149  id = std::atoi(splitted.back().c_str());
3150  if(poses.find(id) == poses.end())
3151  {
3152  UDEBUG("Remove %s", iter->c_str());
3153  _cloudViewer->removeLine(*iter);
3154  }
3155  }
3156  }
3157 
3158  UDEBUG("");
3159  if(stats)
3160  {
3161  stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f));
3162  }
3163 
3164  // update 3D graphes (show all poses)
3165  _cloudViewer->removeAllGraphs();
3166  _cloudViewer->removeCloud("graph_nodes");
3167  if(!_preferencesDialog->isFrustumsShown(0))
3168  {
3169  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
3170  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
3171  {
3172  std::list<std::string> splitted = uSplitNumChar(iter.key());
3173  if(splitted.size() == 2)
3174  {
3175  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0))
3176  {
3177  _cloudViewer->removeFrustum(iter.key());
3178  }
3179  }
3180  }
3181  }
3182 
3183  Transform mapToGt = Transform::getIdentity();
3184  if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
3185  {
3186  mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
3187  }
3188 
3189  std::map<int, Transform> posesWithOdomCache;
3190 
3191  if(_ui->graphicsView_graphView->isVisible() ||
3192  ((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size()))
3193  {
3194  posesWithOdomCache = posesIn;
3195  for(std::map<int, Transform>::const_iterator iter=odomCachePoses.begin(); iter!=odomCachePoses.end(); ++iter)
3196  {
3197  posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second));
3198  }
3199  }
3200 
3201  if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
3202  {
3203  UTimer timerGraph;
3204  // Find all graphs
3205  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
3206  for(std::map<int, Transform>::iterator iter=posesWithOdomCache.lower_bound(1); iter!=posesWithOdomCache.end(); ++iter)
3207  {
3208  int mapId = uValue(_currentMapIds, iter->first, -1);
3209 
3210  if(_preferencesDialog->isGraphsShown())
3211  {
3212  //edges
3213  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
3214  if(kter == graphs.end())
3215  {
3216  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
3217  }
3218  pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
3219  kter->second->push_back(pt);
3220  }
3221 
3222  // get local transforms for frustums on the graph
3223  if(_preferencesDialog->isFrustumsShown(0))
3224  {
3225  std::string frustumId = uFormat("f_%d", iter->first);
3226  if(_cloudViewer->getAddedFrustums().contains(frustumId))
3227  {
3228  _cloudViewer->updateFrustumPose(frustumId, iter->second);
3229  }
3230  else if(_cachedSignatures.contains(iter->first))
3231  {
3232  const Signature & s = _cachedSignatures.value(iter->first);
3233  // Supporting only one frustum per node
3234  if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModels().size()==1)
3235  {
3236  const CameraModel & model = s.sensorData().stereoCameraModels().size()?s.sensorData().stereoCameraModels()[0].left():s.sensorData().cameraModels()[0];
3237  Transform t = model.localTransform();
3238  if(!t.isNull())
3239  {
3240  QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
3241  _cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
3242 
3243  if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
3244  {
3245  std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
3246  color = Qt::gray;
3247  _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
3248  }
3249  }
3250  }
3251  }
3252  }
3253  }
3254 
3255  //Ground truth graph?
3256  for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
3257  {
3258  int mapId = -100;
3259 
3260  if(_preferencesDialog->isGraphsShown())
3261  {
3262  //edges
3263  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
3264  if(kter == graphs.end())
3265  {
3266  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
3267  }
3268  Transform t = mapToGt*iter->second;
3269  pcl::PointXYZ pt(t.x(), t.y(), t.z());
3270  kter->second->push_back(pt);
3271  }
3272  }
3273 
3274  // add graphs
3275  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
3276  {
3277  QColor color = Qt::gray;
3278  if(iter->first >= 0)
3279  {
3280  color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
3281  }
3282  _cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
3283  }
3284 
3285  if(_preferencesDialog->isFrustumsShown(0))
3286  {
3287  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
3288  UDEBUG("remove not used frustums");
3289  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
3290  {
3291  std::list<std::string> splitted = uSplitNumChar(iter.key());
3292  if(splitted.size() == 2)
3293  {
3294  int id = std::atoi(splitted.back().c_str());
3295  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
3296  posesWithOdomCache.find(id) == posesWithOdomCache.end())
3297  {
3298  _cloudViewer->removeFrustum(iter.key());
3299  }
3300  }
3301  }
3302  }
3303 
3304  UDEBUG("timerGraph=%fs", timerGraph.ticks());
3305  }
3306 
3307  UDEBUG("labels.size()=%d", (int)labels.size());
3308 
3309  // Update labels
3310  _cloudViewer->removeAllTexts();
3311  if(_preferencesDialog->isLabelsShown() && labels.size())
3312  {
3313  for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
3314  {
3315  if(nodePoses.find(iter->first)!=nodePoses.end())
3316  {
3317  int mapId = uValue(mapIdsIn, iter->first, -1);
3318  QColor color = Qt::gray;
3319  if(mapId >= 0)
3320  {
3321  color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
3322  }
3323  _cloudViewer->addOrUpdateText(
3324  std::string("label_") + uNumber2Str(iter->first),
3325  iter->second,
3326  _currentPosesMap.at(iter->first),
3327  0.1,
3328  color);
3329  }
3330  }
3331  }
3332 
3333  UDEBUG("");
3334  if(stats)
3335  {
3336  stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f));
3337  }
3338 
3339 #ifdef RTABMAP_OCTOMAP
3340  _cloudViewer->removeOctomap();
3341  _cloudViewer->removeCloud("octomap_cloud");
3342  if(_preferencesDialog->isOctomapUpdated())
3343  {
3344  UDEBUG("");
3345  UTimer time;
3346  _octomap->update(poses);
3347  UINFO("Octomap update time = %fs", time.ticks());
3348  }
3349  if(stats)
3350  {
3351  stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f));
3352  }
3353  if(_preferencesDialog->isOctomapShown())
3354  {
3355  UDEBUG("");
3356  UTimer time;
3357  if(_preferencesDialog->getOctomapRenderingType() > 0)
3358  {
3359  _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
3360  }
3361  else
3362  {
3363  pcl::IndicesPtr obstacles(new std::vector<int>);
3364  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = _octomap->createCloud(_preferencesDialog->getOctomapTreeDepth(), obstacles.get());
3365  if(obstacles->size())
3366  {
3367  _cloudViewer->addCloud("octomap_cloud", cloud);
3368  _cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize());
3369  }
3370  }
3371  UINFO("Octomap show 3d map time = %fs", time.ticks());
3372  }
3373  UDEBUG("");
3374  if(stats)
3375  {
3376  stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f));
3377  }
3378 #endif
3379 
3380 #ifdef RTABMAP_GRIDMAP
3381  _cloudViewer->removeElevationMap();
3382  _cloudViewer->removeCloud("elevation_mesh");
3383  if(_preferencesDialog->getElevationMapShown() > 0)
3384  {
3385  UDEBUG("");
3386  UTimer time;
3387  _elevationMap->update(poses);
3388  UINFO("Elevation map update time = %fs", time.ticks());
3389  }
3390  if(stats)
3391  {
3392  stats->insert(std::make_pair("GUI/Elevation Update/ms", (float)timer.restart()*1000.0f));
3393  }
3394  if(_preferencesDialog->getElevationMapShown() > 0)
3395  {
3396  UDEBUG("");
3397  UTimer time;
3398  if(_preferencesDialog->getElevationMapShown() == 1)
3399  {
3400  float xMin, yMin, cellSize;
3401  cv::Mat map = _elevationMap->createHeightMap(xMin, yMin, cellSize);
3402  if(!map.empty())
3403  {
3404  _cloudViewer->addElevationMap(map, cellSize, xMin, yMin, 1);
3405  }
3406  }
3407  else // RGB elevation
3408  {
3409  pcl::PolygonMesh::Ptr mesh = _elevationMap->createTerrainMesh();
3410  if(mesh->cloud.data.size())
3411  {
3412  _cloudViewer->addCloudMesh("elevation_mesh", mesh);
3413  }
3414  }
3415  UINFO("Show elevation map time = %fs", time.ticks());
3416  }
3417  UDEBUG("");
3418  if(stats)
3419  {
3420  stats->insert(std::make_pair("GUI/Elevation Rendering/ms", (float)timer.restart()*1000.0f));
3421  }
3422 #endif
3423 
3424  // Add landmarks to 3D Map view
3425 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
3426  _cloudViewer->removeAllCoordinates("landmark_");
3427 #endif
3428  if(_preferencesDialog->isLandmarksShown())
3429  {
3430  for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end() && iter->first<0; ++iter)
3431  {
3432 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
3433  _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);
3434 #endif
3435  if(_preferencesDialog->isLabelsShown())
3436  {
3437  std::string num = uNumber2Str(-iter->first);
3438  _cloudViewer->addOrUpdateText(
3439  std::string("landmark_str_") + num,
3440  num,
3441  iter->second,
3442  0.1,
3443  Qt::yellow);
3444  }
3445  }
3446  }
3447 
3448  // Update occupancy grid map in 3D map view and graph view
3449  if(_ui->graphicsView_graphView->isVisible())
3450  {
3451  std::multimap<int, Link> constraintsWithOdomCache;
3452  constraintsWithOdomCache = constraints;
3453  constraintsWithOdomCache.insert(odomCacheConstraints.begin(), odomCacheConstraints.end());
3454  _ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map<int, int>(), uKeysSet(odomCachePoses));
3455  if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
3456  {
3457  std::map<int, Transform> gtPoses = _currentGTPosesMap;
3458  for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
3459  {
3460  iter->second = mapToGt * iter->second;
3461  }
3462  _ui->graphicsView_graphView->updateGTGraph(gtPoses);
3463  }
3464  else
3465  {
3466  _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
3467  }
3468  }
3469  cv::Mat map8U;
3470  if((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
3471  (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown()))
3472  {
3473  float xMin, yMin;
3474  float resolution = _occupancyGrid->getCellSize();
3475  cv::Mat map8S;
3476 #ifdef RTABMAP_OCTOMAP
3477  if(_preferencesDialog->isOctomap2dGrid())
3478  {
3479  map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
3480  }
3481  else
3482 #endif
3483  {
3484  if(_occupancyGrid->addedNodes().size() || _cachedLocalMaps.size()>0)
3485  {
3486  _occupancyGrid->update(poses);
3487  }
3488  if(stats)
3489  {
3490  stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
3491  }
3492  map8S = _occupancyGrid->getMap(xMin, yMin);
3493  }
3494  if(!map8S.empty())
3495  {
3496  //convert to gray scaled map
3497  map8U = util3d::convertMap2Image8U(map8S);
3498 
3499  if(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())
3500  {
3501  float opacity = _preferencesDialog->getGridMapOpacity();
3502  _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
3503  }
3504  if(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible())
3505  {
3506  _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
3507  }
3508  }
3509  }
3510  _ui->graphicsView_graphView->update();
3511 
3512  _cachedLocalMaps.clear(true);
3513 
3514  UDEBUG("");
3515  if(stats)
3516  {
3517  stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f));
3518  }
3519 
3520  if(!_preferencesDialog->getGridMapShown())
3521  {
3522  UDEBUG("");
3523  _cloudViewer->removeOccupancyGridMap();
3524  }
3525 
3526  if(viewerClouds.contains("cloudOdom"))
3527  {
3528  if(!_preferencesDialog->isCloudsShown(1))
3529  {
3530  UDEBUG("");
3531  _cloudViewer->setCloudVisibility("cloudOdom", false);
3532  }
3533  else
3534  {
3535  UDEBUG("");
3536  _cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
3537  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
3538  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
3539  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
3540  }
3541  }
3542  if(viewerClouds.contains("scanOdom"))
3543  {
3544  if(!_preferencesDialog->isScansShown(1))
3545  {
3546  UDEBUG("");
3547  _cloudViewer->setCloudVisibility("scanOdom", false);
3548  }
3549  else
3550  {
3551  UDEBUG("");
3552  _cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
3553  _cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
3554  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
3555  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
3556  }
3557  }
3558  if(viewerClouds.contains("scanMapOdom"))
3559  {
3560  if(!_preferencesDialog->isScansShown(1))
3561  {
3562  UDEBUG("");
3563  _cloudViewer->setCloudVisibility("scanMapOdom", false);
3564  }
3565  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
3566  {
3567  UDEBUG("");
3568  _cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
3569  _cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
3570  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
3571  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
3572  }
3573  }
3574  if(viewerClouds.contains("featuresOdom"))
3575  {
3576  if(!_preferencesDialog->isFeaturesShown(1))
3577  {
3578  UDEBUG("");
3579  _cloudViewer->setCloudVisibility("featuresOdom", false);
3580  }
3581  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
3582  {
3583  UDEBUG("");
3584  _cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
3585  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
3586  }
3587  }
3588 
3589  // activate actions
3590  if(_state != kMonitoring && _state != kDetecting)
3591  {
3592  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
3593  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
3594  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
3595 #ifdef RTABMAP_OCTOMAP
3596  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
3597 #else
3598  _ui->actionExport_octomap->setEnabled(false);
3599 #endif
3600  }
3601 
3602  UDEBUG("");
3603  _cloudViewer->refreshView();
3604  UDEBUG("");
3605 }
3606 
3607 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
3608 {
3609  UASSERT(!pose.isNull());
3610  std::string cloudName = uFormat("cloud%d", nodeId);
3611  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
3612  if(_cloudViewer->getAddedClouds().contains(cloudName))
3613  {
3614  UERROR("Cloud %d already added to map.", nodeId);
3615  return outputPair;
3616  }
3617 
3618  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3619  if(iter == _cachedSignatures.end())
3620  {
3621  UERROR("Node %d is not in the cache.", nodeId);
3622  return outputPair;
3623  }
3624 
3625  UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
3626 
3627  if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
3628  (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
3629  {
3630  cv::Mat image, depth;
3631  SensorData data = iter->sensorData();
3632  data.uncompressData(&image, &depth, 0);
3633  ParametersMap allParameters = _preferencesDialog->getAllParameters();
3634  bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
3635  bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
3636  Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
3637  Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
3638  if(rectifyOnlyFeatures && !imagesAlreadyRectified)
3639  {
3640  if(data.cameraModels().size())
3641  {
3642  UTimer time;
3643  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
3644  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
3645  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
3646  cv::Mat rectifiedImages = data.imageRaw().clone();
3647  bool initRectMaps = _rectCameraModels.empty();
3648  if(initRectMaps)
3649  {
3650  _rectCameraModels.resize(data.cameraModels().size());
3651  }
3652  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
3653  {
3654  if(data.cameraModels()[i].isValidForRectification())
3655  {
3656  if(initRectMaps)
3657  {
3658  _rectCameraModels[i] = data.cameraModels()[i];
3659  if(!_rectCameraModels[i].isRectificationMapInitialized())
3660  {
3661  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
3662  _rectCameraModels[i].initRectificationMap();
3663  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
3664  }
3665  }
3666  UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
3667  _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
3668  cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3669  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3670  }
3671  else
3672  {
3673  UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
3674  i, data.id(),
3675  data.cameraModels()[i].imageWidth(),
3676  data.cameraModels()[i].imageHeight());
3677  }
3678  }
3679  UINFO("Time rectification: %fs", time.ticks());
3680  data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
3681  image = rectifiedImages;
3682  }
3683  }
3684 
3685  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
3686  pcl::IndicesPtr indices(new std::vector<int>);
3687  UASSERT_MSG(nodeId == 0 || nodeId == data.id(), uFormat("nodeId=%d data.id()=%d", nodeId, data.id()).c_str());
3688 
3689  // Create organized cloud
3690  cloud = util3d::cloudRGBFromSensorData(data,
3691  _preferencesDialog->getCloudDecimation(0),
3692  _preferencesDialog->getCloudMaxDepth(0),
3693  _preferencesDialog->getCloudMinDepth(0),
3694  indices.get(),
3695  allParameters,
3696  _preferencesDialog->getCloudRoiRatios(0));
3697 
3698  // view point
3699  Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
3700  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
3701  {
3702  viewPoint[0] = data.cameraModels()[0].localTransform().x();
3703  viewPoint[1] = data.cameraModels()[0].localTransform().y();
3704  viewPoint[2] = data.cameraModels()[0].localTransform().z();
3705  }
3706  else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
3707  {
3708  viewPoint[0] = data.stereoCameraModels()[0].localTransform().x();
3709  viewPoint[1] = data.stereoCameraModels()[0].localTransform().y();
3710  viewPoint[2] = data.stereoCameraModels()[0].localTransform().z();
3711  }
3712 
3713  // filtering pipeline
3714  if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
3715  {
3716  cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
3717  //generate indices for all points (they are all valid)
3718  indices->resize(cloud->size());
3719  for(unsigned int i=0; i<cloud->size(); ++i)
3720  {
3721  indices->at(i) = i;
3722  }
3723  }
3724 
3725  // Do ceiling/floor filtering
3726  if(indices->size() &&
3727  (_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
3728  _preferencesDialog->getCeilingFilteringHeight() != 0.0))
3729  {
3730  // perform in /map frame
3731  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
3732  indices = rtabmap::util3d::passThrough(
3733  cloudTransformed,
3734  indices,
3735  "z",
3736  _preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
3737  _preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
3738  }
3739 
3740  // Do radius filtering after voxel filtering ( a lot faster)
3741  if(indices->size() &&
3742  _preferencesDialog->getNoiseRadius() > 0.0 &&
3743  _preferencesDialog->getNoiseMinNeighbors() > 0)
3744  {
3745  indices = rtabmap::util3d::radiusFiltering(
3746  cloud,
3747  indices,
3748  _preferencesDialog->getNoiseRadius(),
3749  _preferencesDialog->getNoiseMinNeighbors());
3750  }
3751 
3752  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3753  if(_preferencesDialog->isSubtractFiltering() &&
3754  _preferencesDialog->getSubtractFilteringRadius() > 0.0 &&
3755  nodeId > 0)
3756  {
3757  pcl::IndicesPtr beforeFiltering = indices;
3758  if( cloud->size() &&
3759  _previousCloud.first>0 &&
3760  _previousCloud.second.first.first.get() != 0 &&
3761  _previousCloud.second.second.get() != 0 &&
3762  _previousCloud.second.second->size() &&
3763  _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
3764  {
3765  UTimer time;
3766 
3767  rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
3768 
3769  //UWARN("saved new.pcd and old.pcd");
3770  //pcl::io::savePCDFile("new.pcd", *cloud, *indices);
3771  //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
3772 
3773  if(_preferencesDialog->isSubtractFiltering())
3774  {
3775  if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
3776  {
3777  //normals required
3778  if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
3779  {
3780  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
3781  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3782  }
3783  else
3784  {
3785  UWARN("Cloud subtraction with angle filtering is activated but "
3786  "cloud normal K search is 0. Subtraction is done with angle.");
3787  }
3788  }
3789 
3790  if(cloudWithNormals->size() &&
3791  _previousCloud.second.first.second.get() &&
3792  _previousCloud.second.first.second->size())
3793  {
3794  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t);
3795  indices = rtabmap::util3d::subtractFiltering(
3796  cloudWithNormals,
3797  indices,
3798  previousCloud,
3799  _previousCloud.second.second,
3800  _preferencesDialog->getSubtractFilteringRadius(),
3801  _preferencesDialog->getSubtractFilteringAngle(),
3802  _preferencesDialog->getSubtractFilteringMinPts());
3803  }
3804  else
3805  {
3806  pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t);
3807  indices = rtabmap::util3d::subtractFiltering(
3808  cloud,
3809  indices,
3810  previousCloud,
3811  _previousCloud.second.second,
3812  _preferencesDialog->getSubtractFilteringRadius(),
3813  _preferencesDialog->getSubtractFilteringMinPts());
3814  }
3815 
3816 
3817  UINFO("Time subtract filtering %d from %d -> %d (%fs)",
3818  (int)_previousCloud.second.second->size(),
3819  (int)beforeFiltering->size(),
3820  (int)indices->size(),
3821  time.ticks());
3822  }
3823  }
3824  // keep all indices for next subtraction
3825  _previousCloud.first = nodeId;
3826  _previousCloud.second.first.first = cloud;
3827  _previousCloud.second.first.second = cloudWithNormals;
3828  _previousCloud.second.second = beforeFiltering;
3829  }
3830 
3831  if(indices->size())
3832  {
3833  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
3834  bool added = false;
3835  if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
3836  {
3837  // Fast organized mesh
3838  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
3839  output = util3d::extractIndices(cloud, indices, false, true);
3840  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
3841  output,
3842  _preferencesDialog->getCloudMeshingAngle(),
3843  _preferencesDialog->isCloudMeshingQuad(),
3844  _preferencesDialog->getCloudMeshingTriangleSize(),
3845  viewPoint);
3846  if(polygons.size())
3847  {
3848  // remove unused vertices to save memory
3849  pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
3850  std::vector<pcl::Vertices> outputPolygons;
3851  std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
3852 
3853  if(_preferencesDialog->isCloudMeshingTexture() && !image.empty())
3854  {
3855  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
3856  pcl::toPCLPointCloud2(*outputFiltered, textureMesh->cloud);
3857  textureMesh->tex_polygons.push_back(outputPolygons);
3858  int w = cloud->width;
3859  int h = cloud->height;
3860  UASSERT(w > 1 && h > 1);
3861  textureMesh->tex_coordinates.resize(1);
3862  int nPoints = (int)outputFiltered->size();
3863  textureMesh->tex_coordinates[0].resize(nPoints);
3864  for(int i=0; i<nPoints; ++i)
3865  {
3866  //uv
3867  UASSERT(i < (int)denseToOrganizedIndices.size());
3868  int originalVertex = denseToOrganizedIndices[i];
3869  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
3870  float(originalVertex % w) / float(w), // u
3871  float(h - originalVertex / w) / float(h)); // v
3872  }
3873 
3874  pcl::TexMaterial mesh_material;
3875  mesh_material.tex_d = 1.0f;
3876  mesh_material.tex_Ns = 75.0f;
3877  mesh_material.tex_illum = 1;
3878 
3879  std::stringstream tex_name;
3880  tex_name << "material_" << nodeId;
3881  tex_name >> mesh_material.tex_name;
3882 
3883  mesh_material.tex_file = "";
3884 
3885  textureMesh->tex_materials.push_back(mesh_material);
3886 
3887  if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, image, pose))
3888  {
3889  UERROR("Adding texture mesh %d to viewer failed!", nodeId);
3890  }
3891  else
3892  {
3893  added = true;
3894  }
3895  }
3896  else if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
3897  {
3898  UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
3899  }
3900  else
3901  {
3902  added = true;
3903  }
3904  }
3905  }
3906  else
3907  {
3908  if(_preferencesDialog->isCloudMeshing())
3909  {
3910  UWARN("Online meshing is activated but the generated cloud is "
3911  "dense (voxel filtering is used or multiple cameras are used). Disable "
3912  "online meshing in Preferences->3D Rendering to hide this warning.");
3913  }
3914 
3915  if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
3916  {
3917  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
3918  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3919  }
3920 
3921  QColor color = Qt::gray;
3922  if(mapId >= 0)
3923  {
3924  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3925  }
3926 
3927  output = util3d::extractIndices(cloud, indices, false, true);
3928 
3929  if(cloudWithNormals->size())
3930  {
3931  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
3932  outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false);
3933 
3934  if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color))
3935  {
3936  UERROR("Adding cloud %d to viewer failed!", nodeId);
3937  }
3938  else
3939  {
3940  added = true;
3941  }
3942  }
3943  else
3944  {
3945  if(!_cloudViewer->addCloud(cloudName, output, pose, color))
3946  {
3947  UERROR("Adding cloud %d to viewer failed!", nodeId);
3948  }
3949  else
3950  {
3951  added = true;
3952  }
3953  }
3954  }
3955  if(added)
3956  {
3957  outputPair.first = output;
3958  outputPair.second = indices;
3959  if(_preferencesDialog->isCloudsKept() && nodeId > 0)
3960  {
3961  _cachedClouds.insert(std::make_pair(nodeId, outputPair));
3962  _createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
3963  }
3964  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
3965  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
3966  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
3967  }
3968  else if(nodeId>0)
3969  {
3970  _cachedEmptyClouds.insert(nodeId);
3971  }
3972  }
3973  else if(nodeId>0)
3974  {
3975  _cachedEmptyClouds.insert(nodeId);
3976  }
3977  }
3978 
3979  return outputPair;
3980 }
3981 
3982 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
3983 {
3984  std::string scanName = uFormat("scan%d", nodeId);
3985  if(_cloudViewer->getAddedClouds().contains(scanName))
3986  {
3987  UERROR("Scan %d already added to map.", nodeId);
3988  return;
3989  }
3990 
3991  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3992  if(iter == _cachedSignatures.end())
3993  {
3994  UERROR("Node %d is not in the cache.", nodeId);
3995  return;
3996  }
3997 
3998  if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
3999  {
4000  LaserScan scan;
4001  iter->sensorData().uncompressData(0, 0, &scan);
4002 
4003  if(_preferencesDialog->getDownsamplingStepScan(0) > 1 ||
4004  _preferencesDialog->getScanMaxRange(0) > 0.0f ||
4005  _preferencesDialog->getScanMinRange(0) > 0.0f)
4006  {
4007  scan = util3d::commonFiltering(scan,
4008  _preferencesDialog->getDownsamplingStepScan(0),
4009  _preferencesDialog->getScanMinRange(0),
4010  _preferencesDialog->getScanMaxRange(0));
4011  }
4012 
4013  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
4014  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
4015  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
4016  pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
4017  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
4018  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
4019  if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
4020  {
4021  cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
4022  }
4023  else if(scan.hasNormals() && scan.hasIntensity() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
4024  {
4025  cloudIWithNormals = util3d::laserScanToPointCloudINormal(scan, scan.localTransform());
4026  }
4027  else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
4028  {
4029  cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
4030  }
4031  else if(scan.hasRGB())
4032  {
4033  cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
4034  }
4035  else if(scan.hasIntensity())
4036  {
4037  cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
4038  }
4039  else
4040  {
4041  cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
4042  }
4043 
4044  if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
4045  {
4046  if(cloud.get())
4047  {
4048  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
4049  }
4050  if(cloudRGB.get())
4051  {
4052  cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
4053  }
4054  if(cloudI.get())
4055  {
4056  cloudI = util3d::voxelize(cloudI, _preferencesDialog->getCloudVoxelSizeScan(0));
4057  }
4058  }
4059 
4060  // Do ceiling/floor filtering
4061  if((!scan.is2d()) && // don't filter 2D scans
4062  (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
4063  _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
4064  {
4065  if(cloudRGBWithNormals.get())
4066  {
4067  // perform in /map frame
4068  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
4069  cloudTransformed = rtabmap::util3d::passThrough(
4070  cloudTransformed,
4071  "z",
4072  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4073  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4074 
4075  //transform back in sensor frame
4076  cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4077  }
4078  if(cloudIWithNormals.get())
4079  {
4080  // perform in /map frame
4081  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudIWithNormals, pose);
4082  cloudTransformed = rtabmap::util3d::passThrough(
4083  cloudTransformed,
4084  "z",
4085  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4086  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4087 
4088  //transform back in sensor frame
4089  cloudIWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4090  }
4091  if(cloudWithNormals.get())
4092  {
4093  // perform in /map frame
4094  pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
4095  cloudTransformed = rtabmap::util3d::passThrough(
4096  cloudTransformed,
4097  "z",
4098  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4099  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4100 
4101  //transform back in sensor frame
4102  cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4103  }
4104  if(cloudRGB.get())
4105  {
4106  // perform in /map frame
4107  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
4108  cloudTransformed = rtabmap::util3d::passThrough(
4109  cloudTransformed,
4110  "z",
4111  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4112  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4113 
4114  //transform back in sensor frame
4115  cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4116  }
4117  if(cloudI.get())
4118  {
4119  // perform in /map frame
4120  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudTransformed = util3d::transformPointCloud(cloudI, pose);
4121  cloudTransformed = rtabmap::util3d::passThrough(
4122  cloudTransformed,
4123  "z",
4124  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4125  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4126 
4127  //transform back in sensor frame
4128  cloudI = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4129  }
4130  if(cloud.get())
4131  {
4132  // perform in /map frame
4133  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
4134  cloudTransformed = rtabmap::util3d::passThrough(
4135  cloudTransformed,
4136  "z",
4137  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
4138  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
4139 
4140  //transform back in sensor frame
4141  cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
4142  }
4143  }
4144 
4145  if( (cloud.get() || cloudRGB.get() || cloudI.get()) &&
4146  (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
4147  {
4148  Eigen::Vector3f scanViewpoint(
4149  scan.localTransform().x(),
4150  scan.localTransform().y(),
4151  scan.localTransform().z());
4152 
4153  pcl::PointCloud<pcl::Normal>::Ptr normals;
4154  if(cloud.get() && cloud->size())
4155  {
4156  if(scan.is2d())
4157  {
4158  normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
4159  }
4160  else
4161  {
4162  normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
4163  }
4164  cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
4165  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
4166  cloud.reset();
4167  }
4168  else if(cloudRGB.get() && cloudRGB->size())
4169  {
4170  // Assuming 3D
4171  normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
4172  cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
4173  pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
4174  cloudRGB.reset();
4175  }
4176  else if(cloudI.get())
4177  {
4178  if(scan.is2d())
4179  {
4180  normals = util3d::computeFastOrganizedNormals2D(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
4181  }
4182  else
4183  {
4184  normals = util3d::computeNormals(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
4185  }
4186  cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
4187  pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
4188  cloudI.reset();
4189  }
4190  }
4191 
4192  QColor color = Qt::gray;
4193  if(mapId >= 0)
4194  {
4195  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
4196  }
4197  bool added = false;
4198  if(cloudRGBWithNormals.get())
4199  {
4200  added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
4201  if(added && nodeId > 0)
4202  {
4203  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4204  }
4205  }
4206  else if(cloudIWithNormals.get())
4207  {
4208  added = _cloudViewer->addCloud(scanName, cloudIWithNormals, pose, color);
4209  if(added && nodeId > 0)
4210  {
4211  if(scan.is2d())
4212  {
4213  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4214  }
4215  else
4216  {
4217  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4218  }
4219  }
4220  }
4221  else if(cloudWithNormals.get())
4222  {
4223  added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
4224  if(added && nodeId > 0)
4225  {
4226  if(scan.is2d())
4227  {
4228  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4229  }
4230  else
4231  {
4232  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4233  }
4234  }
4235  }
4236  else if(cloudRGB.get())
4237  {
4238  added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
4239  if(added && nodeId > 0)
4240  {
4241  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4242  }
4243  }
4244  else if(cloudI.get())
4245  {
4246  added = _cloudViewer->addCloud(scanName, cloudI, pose, color);
4247  if(added && nodeId > 0)
4248  {
4249  if(scan.is2d())
4250  {
4251  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4252  }
4253  else
4254  {
4255  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4256  }
4257  }
4258  }
4259  else
4260  {
4261  UASSERT(cloud.get());
4262  added = _cloudViewer->addCloud(scanName, cloud, pose, color);
4263  if(added && nodeId > 0)
4264  {
4265  if(scan.is2d())
4266  {
4267  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4268  }
4269  else
4270  {
4271  scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
4272  }
4273  }
4274  }
4275  if(!added)
4276  {
4277  UERROR("Adding cloud %d to viewer failed!", nodeId);
4278  }
4279  else
4280  {
4281  if(nodeId > 0)
4282  {
4283  _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in scan frame
4284  }
4285 
4286  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(0));
4287  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
4288  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
4289  }
4290  }
4291 }
4292 
4293 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
4294 {
4295  UDEBUG("");
4296  UASSERT(!pose.isNull());
4297  std::string cloudName = uFormat("features%d", nodeId);
4298  if(_cloudViewer->getAddedClouds().contains(cloudName))
4299  {
4300  UERROR("Features cloud %d already added to map.", nodeId);
4301  return;
4302  }
4303 
4304  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
4305  if(iter == _cachedSignatures.end())
4306  {
4307  UERROR("Node %d is not in the cache.", nodeId);
4308  return;
4309  }
4310 
4311  if(_createdFeatures.find(nodeId) != _createdFeatures.end())
4312  {
4313  UDEBUG("Features cloud %d already created.");
4314  return;
4315  }
4316 
4317  if(iter->getWords3().size())
4318  {
4319  UINFO("Create cloud from 3D words");
4320  QColor color = Qt::gray;
4321  if(mapId >= 0)
4322  {
4323  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
4324  }
4325 
4326  cv::Mat rgb;
4327  if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
4328  {
4329  SensorData data = iter->sensorData();
4330  data.uncompressData(&rgb, 0, 0);
4331  }
4332 
4333  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
4334  cloud->resize(iter->getWords3().size());
4335  int oi=0;
4336  UASSERT(iter->getWords().size() == iter->getWords3().size());
4337  float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
4338  UDEBUG("rgb.channels()=%d");
4339  if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
4340  {
4341  Transform invLocalTransform = Transform::getIdentity();
4342  if(iter.value().sensorData().cameraModels().size() == 1 &&
4343  iter.value().sensorData().cameraModels().at(0).isValidForProjection())
4344  {
4345  invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
4346  }
4347  else if(iter.value().sensorData().stereoCameraModels().size() == 1 &&
4348  iter.value().sensorData().stereoCameraModels()[0].isValidForProjection())
4349  {
4350  invLocalTransform = iter.value().sensorData().stereoCameraModels()[0].left().localTransform().inverse();
4351  }
4352 
4353  for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
4354  {
4355  const cv::Point3f & pt = iter->getWords3()[jter->second];
4356  if(util3d::isFinite(pt) &&
4357  (maxDepth == 0.0f ||
4358  //move back point in camera frame (to get depth along z), ignore for multi-camera
4359  (iter.value().sensorData().cameraModels().size()<=1 &&
4360  util3d::transformPoint(pt, invLocalTransform).z < maxDepth)))
4361  {
4362  (*cloud)[oi].x = pt.x;
4363  (*cloud)[oi].y = pt.y;
4364  (*cloud)[oi].z = pt.z;
4365  const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
4366  int u = kpt.pt.x+0.5;
4367  int v = kpt.pt.y+0.5;
4368  if(!rgb.empty() &&
4369  uIsInBounds(u, 0, rgb.cols-1) &&
4370  uIsInBounds(v, 0, rgb.rows-1))
4371  {
4372  if(rgb.channels() == 1)
4373  {
4374  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
4375  }
4376  else
4377  {
4378  cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
4379  (*cloud)[oi].b = bgr.val[0];
4380  (*cloud)[oi].g = bgr.val[1];
4381  (*cloud)[oi].r = bgr.val[2];
4382  }
4383  }
4384  else
4385  {
4386  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
4387  }
4388  ++oi;
4389  }
4390  }
4391  }
4392  cloud->resize(oi);
4393  if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
4394  {
4395  UERROR("Adding features cloud %d to viewer failed!", nodeId);
4396  }
4397  else if(nodeId > 0)
4398  {
4399  _createdFeatures.insert(std::make_pair(nodeId, cloud));
4400  }
4401  }
4402  else
4403  {
4404  return;
4405  }
4406 
4407  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
4408  UDEBUG("");
4409 }
4410 
4411 Transform MainWindow::alignPosesToGroundTruth(
4412  const std::map<int, Transform> & poses,
4413  const std::map<int, Transform> & groundTruth)
4414 {
4415  Transform t = Transform::getIdentity();
4416  if(groundTruth.size() && poses.size())
4417  {
4418  float translational_rmse = 0.0f;
4419  float translational_mean = 0.0f;
4420  float translational_median = 0.0f;
4421  float translational_std = 0.0f;
4422  float translational_min = 0.0f;
4423  float translational_max = 0.0f;
4424  float rotational_rmse = 0.0f;
4425  float rotational_mean = 0.0f;
4426  float rotational_median = 0.0f;
4427  float rotational_std = 0.0f;
4428  float rotational_min = 0.0f;
4429  float rotational_max = 0.0f;
4430 
4431  t = graph::calcRMSE(
4432  groundTruth,
4433  poses,
4434  translational_rmse,
4435  translational_mean,
4436  translational_median,
4437  translational_std,
4438  translational_min,
4439  translational_max,
4440  rotational_rmse,
4441  rotational_mean,
4442  rotational_median,
4443  rotational_std,
4444  rotational_min,
4445  rotational_max);
4446 
4447  // ground truth live statistics
4448  UINFO("translational_rmse=%f", translational_rmse);
4449  UINFO("translational_mean=%f", translational_mean);
4450  UINFO("translational_median=%f", translational_median);
4451  UINFO("translational_std=%f", translational_std);
4452  UINFO("translational_min=%f", translational_min);
4453  UINFO("translational_max=%f", translational_max);
4454 
4455  UINFO("rotational_rmse=%f", rotational_rmse);
4456  UINFO("rotational_mean=%f", rotational_mean);
4457  UINFO("rotational_median=%f", rotational_median);
4458  UINFO("rotational_std=%f", rotational_std);
4459  UINFO("rotational_min=%f", rotational_min);
4460  UINFO("rotational_max=%f", rotational_max);
4461  }
4462  return t;
4463 }
4464 
4465 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
4466 {
4467  UINFO("Update visibility %d", nodeId);
4468  QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
4469  Transform pose;
4470  if(_currentGTPosesMap.size() &&
4471  _ui->actionAnchor_clouds_to_ground_truth->isChecked() &&
4472  _currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end())
4473  {
4474  pose = _currentGTPosesMap.at(nodeId);
4475  }
4476  else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
4477  {
4478  pose = _currentPosesMap.at(nodeId);
4479  }
4480 
4481  if(!pose.isNull() || !visible)
4482  {
4483  if(_preferencesDialog->isCloudsShown(0))
4484  {
4485  std::string cloudName = uFormat("cloud%d", nodeId);
4486  if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
4487  {
4488  createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
4489  }
4490  else if(viewerClouds.contains(cloudName))
4491  {
4492  if(visible)
4493  {
4494  //make sure the transformation was done
4495  _cloudViewer->updateCloudPose(cloudName, pose);
4496  }
4497  _cloudViewer->setCloudVisibility(cloudName, visible);
4498  }
4499  }
4500 
4501  if(_preferencesDialog->isScansShown(0))
4502  {
4503  std::string scanName = uFormat("scan%d", nodeId);
4504  if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
4505  {
4506  createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
4507  }
4508  else if(viewerClouds.contains(scanName))
4509  {
4510  if(visible)
4511  {
4512  //make sure the transformation was done
4513  _cloudViewer->updateCloudPose(scanName, pose);
4514  }
4515  _cloudViewer->setCloudVisibility(scanName, visible);
4516  }
4517  }
4518 
4519  _cloudViewer->refreshView();
4520  }
4521 }
4522 
4523 void MainWindow::updateGraphView()
4524 {
4525  if(_ui->dockWidget_graphViewer->isVisible())
4526  {
4527  UDEBUG("Graph visible!");
4528  if(_currentPosesMap.size())
4529  {
4530  this->updateMapCloud(
4531  std::map<int, Transform>(_currentPosesMap),
4532  std::multimap<int, Link>(_currentLinksMap),
4533  std::map<int, int>(_currentMapIds),
4534  std::map<int, std::string>(_currentLabels),
4535  std::map<int, Transform>(_currentGTPosesMap));
4536  }
4537  }
4538 }
4539 
4540 void MainWindow::processRtabmapEventInit(int status, const QString & info)
4541 {
4542  if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
4543  {
4544  _progressDialog->resetProgress();
4545  _progressDialog->show();
4546  this->changeState(MainWindow::kInitializing);
4547  }
4548  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
4549  {
4550  _progressDialog->setValue(_progressDialog->maximumSteps());
4551  this->changeState(MainWindow::kInitialized);
4552 
4553  if(!_openedDatabasePath.isEmpty())
4554  {
4555  this->downloadAllClouds();
4556  }
4557  }
4558  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
4559  {
4560  _progressDialog->resetProgress();
4561  _progressDialog->show();
4562  if(_state!=kApplicationClosing)
4563  {
4564  this->changeState(MainWindow::kClosing);
4565  }
4566  }
4567  else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
4568  {
4569  _progressDialog->setValue(_progressDialog->maximumSteps());
4570 
4571  if(_databaseUpdated)
4572  {
4573  if(!_newDatabasePath.isEmpty())
4574  {
4575  if(!_newDatabasePathOutput.isEmpty())
4576  {
4577  bool removed = true;
4578  if(QFile::exists(_newDatabasePathOutput))
4579  {
4580  removed = QFile::remove(_newDatabasePathOutput);
4581  }
4582  if(removed)
4583  {
4584  if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
4585  {
4586  std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
4587  UINFO(msg.c_str());
4588  QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
4589  }
4590  else
4591  {
4592  std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".",
4593  _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
4594  UERROR(msg.c_str());
4595  QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
4596  }
4597  }
4598  else
4599  {
4600  std::string msg = uFormat("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
4601  _newDatabasePathOutput.toStdString().c_str(), _newDatabasePath.toStdString().c_str());
4602  UERROR(msg.c_str());
4603  QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
4604  }
4605  }
4606  else if(QFile::remove(_newDatabasePath))
4607  {
4608  UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
4609  }
4610  else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
4611  {
4612  UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
4613  }
4614 
4615  }
4616  else if(!_openedDatabasePath.isEmpty())
4617  {
4618  std::string msg = uFormat("Database \"%s\" updated.", _openedDatabasePath.toStdString().c_str());
4619  UINFO(msg.c_str());
4620  QMessageBox::information(this, tr("Database updated!"), QString(msg.c_str()));
4621  }
4622  }
4623  else if(!_newDatabasePath.isEmpty())
4624  {
4625  // just remove temporary database;
4626  if(QFile::remove(_newDatabasePath))
4627  {
4628  UINFO("Deleted temporary database \"%s\".", _newDatabasePath.toStdString().c_str());
4629  }
4630  else if(!uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kDbSqlite3InMemory())))
4631  {
4632  UERROR("Temporary database \"%s\" could not be deleted.", _newDatabasePath.toStdString().c_str());
4633  }
4634  }
4635  _openedDatabasePath.clear();
4636  _newDatabasePath.clear();
4637  _newDatabasePathOutput.clear();
4638  bool applicationClosing = _state == kApplicationClosing;
4639  this->changeState(MainWindow::kIdle);
4640  if(applicationClosing)
4641  {
4642  this->close();
4643  }
4644  }
4645  else
4646  {
4647  _progressDialog->incrementStep();
4648  QString msg(info);
4649  if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
4650  {
4651  _openedDatabasePath.clear();
4652  _newDatabasePath.clear();
4653  _newDatabasePathOutput.clear();
4654  _progressDialog->setAutoClose(false);
4655  msg.prepend(tr("[ERROR] "));
4656  _progressDialog->appendText(msg);
4657  this->changeState(MainWindow::kIdle);
4658  }
4659  else
4660  {
4661  _progressDialog->appendText(msg);
4662  }
4663  }
4664 }
4665 
4666 void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event)
4667 {
4668  _progressDialog->appendText("Downloading the map... done.");
4669  _progressDialog->incrementStep();
4670 
4671  if(event.getCode())
4672  {
4673  UERROR("Map received with code error %d!", event.getCode());
4674  _progressDialog->appendText(uFormat("[ERROR] Map received with code error %d!", event.getCode()).c_str());
4675  _progressDialog->setAutoClose(false);
4676  }
4677  else
4678  {
4679 
4680  _processingDownloadedMap = true;
4681  UINFO("Received map!");
4682  _progressDialog->appendText(tr(" poses = %1").arg(event.getPoses().size()));
4683  _progressDialog->appendText(tr(" constraints = %1").arg(event.getConstraints().size()));
4684 
4685  _progressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
4686  _progressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
4687  QApplication::processEvents();
4688 
4689  int addedSignatures = 0;
4690  std::map<int, int> mapIds;
4691  std::map<int, Transform> groundTruth;
4692  std::map<int, std::string> labels;
4693  for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
4694  iter!=event.getSignatures().end();
4695  ++iter)
4696  {
4697  mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
4698  if(!iter->second.getGroundTruthPose().isNull())
4699  {
4700  groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
4701  }
4702  if(!iter->second.getLabel().empty())
4703  {
4704  labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
4705  }
4706  if(!_cachedSignatures.contains(iter->first) ||
4707  (_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty()))
4708  {
4709  _cachedSignatures.insert(iter->first, iter->second);
4710  _cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
4711  unsigned int count = 0;
4712  if(!iter->second.getWords3().empty())
4713  {
4714  for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
4715  {
4716  if(util3d::isFinite(iter->second.getWords3()[jter->second]))
4717  {
4718  ++count;
4719  }
4720  }
4721  }
4722  _cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
4723  ++addedSignatures;
4724  }
4725  _progressDialog->incrementStep();
4726  QApplication::processEvents();
4727  }
4728  _progressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
4729  _progressDialog->incrementStep();
4730  QApplication::processEvents();
4731 
4732  _progressDialog->appendText("Inserting data in the cache... done.");
4733 
4734  if(event.getPoses().size())
4735  {
4736  _progressDialog->appendText("Updating the 3D map cloud...");
4737  _progressDialog->incrementStep();
4738  _progressDialog->setCancelButtonVisible(true);
4739  _progressCanceled = false;
4740  QApplication::processEvents();
4741  std::map<int, Transform> poses = event.getPoses();
4742  this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, std::map<int, Transform>(), std::multimap<int, Link>(), true);
4743 
4744  if( _ui->graphicsView_graphView->isVisible() &&
4745  _preferencesDialog->isWordsCountGraphView() &&
4746  _preferencesDialog->isRGBDMode()&&
4747  _cachedWordsCount.size())
4748  {
4749  _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
4750  }
4751 
4752  _progressDialog->appendText("Updating the 3D map cloud... done.");
4753  }
4754  else
4755  {
4756  _progressDialog->appendText("No poses received! The map cloud cannot be updated...");
4757  UINFO("Map received is empty! Cannot update the map cloud...");
4758  }
4759 
4760  _progressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
4761 
4762  if(!_preferencesDialog->isImagesKept())
4763  {
4764  _cachedSignatures.clear();
4765  _cachedMemoryUsage = 0;
4766  _cachedWordsCount.clear();
4767  }
4768  if(_state != kMonitoring && _state != kDetecting)
4769  {
4770  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
4771  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
4772  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
4773  }
4774  _processingDownloadedMap = false;
4775  }
4776  _progressDialog->setValue(_progressDialog->maximumSteps());
4777  _progressDialog->setCancelButtonVisible(false);
4778  _progressCanceled = false;
4779 
4780  Q_EMIT(rtabmapEvent3DMapProcessed());
4781 }
4782 
4783 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
4784 {
4785  if(!event.getPoses().empty())
4786  {
4787  _ui->graphicsView_graphView->setGlobalPath(event.getPoses());
4788  }
4789 
4790  if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
4791  {
4792  _ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0), _preferencesDialog->isCacheSavedInFigures());
4793  _ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0), _preferencesDialog->isCacheSavedInFigures());
4794  _ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()), _preferencesDialog->isCacheSavedInFigures());
4795  _ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()), _preferencesDialog->isCacheSavedInFigures());
4796  _ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())), _preferencesDialog->isCacheSavedInFigures());
4797  }
4798  if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
4799  {
4800  // use MessageBox
4801  if(event.getPoses().empty())
4802  {
4803  QMessageBox * warn = new QMessageBox(
4804  QMessageBox::Warning,
4805  tr("Setting goal failed!"),
4806  tr("Setting goal to location %1%2 failed. "
4807  "Some reasons: \n"
4808  "1) the robot is not yet localized in the map,\n"
4809  "2) the location doesn't exist in the map,\n"
4810  "3) the location is not linked to the global map or\n"
4811  "4) the location is too near of the current location (goal already reached).")
4812  .arg(event.getGoal())
4813  .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):""),
4814  QMessageBox::Ok,
4815  this);
4816  warn->setAttribute(Qt::WA_DeleteOnClose, true);
4817  warn->show();
4818  }
4819  else
4820  {
4821  QMessageBox * info = new QMessageBox(
4822  QMessageBox::Information,
4823  tr("Goal detected!"),
4824  tr("Global path computed to %1%2 (%3 poses, %4 m).")
4825  .arg(event.getGoal())
4826  .arg(!event.getGoalLabel().empty()?QString(" \"%1\"").arg(event.getGoalLabel().c_str()):"")
4827  .arg(event.getPoses().size())
4828  .arg(graph::computePathLength(event.getPoses())),
4829  QMessageBox::Ok,
4830  this);
4831  info->setAttribute(Qt::WA_DeleteOnClose, true);
4832  info->show();
4833  }
4834  }
4835  else if(event.getPoses().empty() && _waypoints.size())
4836  {
4837  // resend the same goal
4838  QTimer::singleShot(1000, this, SLOT(postGoal()));
4839  }
4840 }
4841 
4842 void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
4843 {
4844  QMessageBox * warn = new QMessageBox(
4845  QMessageBox::Warning,
4846  tr("Setting label failed!"),
4847  tr("Setting label %1 to location %2 failed. "
4848  "Some reasons: \n"
4849  "1) the location doesn't exist in the map,\n"
4850  "2) the location has already a label.").arg(label).arg(id),
4851  QMessageBox::Ok,
4852  this);
4853  warn->setAttribute(Qt::WA_DeleteOnClose, true);
4854  warn->show();
4855 }
4856 
4857 void MainWindow::processRtabmapGoalStatusEvent(int status)
4858 {
4859  _ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
4860  if(_waypoints.size())
4861  {
4862  this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
4863  }
4864 }
4865 
4866 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
4867 {
4868  ULOGGER_DEBUG("");
4869  if(flags & PreferencesDialog::kPanelSource)
4870  {
4871  // Camera settings...
4872  _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
4873  this->updateSelectSourceMenu();
4874  _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
4875 
4876  if(_sensorCapture)
4877  {
4878  if(dynamic_cast<DBReader*>(_sensorCapture->camera()) != 0)
4879  {
4880  _sensorCapture->setFrameRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
4881  }
4882  else
4883  {
4884  _sensorCapture->setFrameRate(_preferencesDialog->getGeneralInputRate());
4885  }
4886  }
4887 
4888  }//This will update the statistics toolbox
4889 
4890  if(flags & PreferencesDialog::kPanelGeneral)
4891  {
4892  UDEBUG("General settings changed...");
4893  setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
4894  if(!_preferencesDialog->isLocalizationsCountGraphView())
4895  {
4896  _cachedLocalizationsCount.clear();
4897  }
4898  if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
4899  {
4900  _ui->graphicsView_graphView->clearPosterior();
4901  }
4902  }
4903 
4904  if(flags & PreferencesDialog::kPanelCloudRendering)
4905  {
4906  UDEBUG("Cloud rendering settings changed...");
4907  if(_currentPosesMap.size())
4908  {
4909  this->updateMapCloud(
4910  std::map<int, Transform>(_currentPosesMap),
4911  std::multimap<int, Link>(_currentLinksMap),
4912  std::map<int, int>(_currentMapIds),
4913  std::map<int, std::string>(_currentLabels),
4914  std::map<int, Transform>(_currentGTPosesMap));
4915  }
4916  }
4917 
4918  if(flags & PreferencesDialog::kPanelLogging)
4919  {
4920  UDEBUG("Logging settings changed...");
4921  ULogger::setLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerLevel());
4922  ULogger::setEventLevel((ULogger::Level)_preferencesDialog->getGeneralLoggerEventLevel());
4923  ULogger::setType((ULogger::Type)_preferencesDialog->getGeneralLoggerType(),
4924  (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
4925  ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
4926  ULogger::setPrintThreadId(_preferencesDialog->getGeneralLoggerPrintThreadId());
4927  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
4928  }
4929 }
4930 
4931 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
4932 {
4933  applyPrefSettings(parameters, true); //post parameters
4934 }
4935 
4936 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
4937 {
4938  ULOGGER_DEBUG("");
4939  if(parameters.size())
4940  {
4941  for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
4942  {
4943  UDEBUG("Parameter changed: Key=%s Value=%s", iter->first.c_str(), iter->second.c_str());
4944  }
4945 
4946  rtabmap::ParametersMap parametersModified = parameters;
4947 
4948  if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
4949  {
4950  _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4951  _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4952  _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
4953  }
4954 
4955  if(_state != kIdle && parametersModified.size())
4956  {
4957  if(postParamEvent)
4958  {
4959  this->post(new ParamEvent(parametersModified));
4960  }
4961  }
4962 
4963  // update loop closure viewer parameters (Use Map parameters)
4964  _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
4965  _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
4966 
4967  // update graph view parameters
4968  if(uContains(parameters, Parameters::kRGBDLocalRadius()))
4969  {
4970  _ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
4971  }
4972  }
4973 
4974  //update ui
4975  _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
4976  _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
4977  _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
4978 
4979  Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
4980 }
4981 
4982 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
4983 {
4984  UTimer timer;
4985 
4986  timer.start();
4987  ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
4988  if(refWords.size())
4989  {
4990  _ui->imageView_source->clearFeatures();
4991  }
4992  for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
4993  {
4994  int id = iter->first;
4995  QColor color;
4996  if(id<0)
4997  {
4998  // GRAY = NOT QUANTIZED
4999  color = Qt::gray;
5000  }
5001  else if(uContains(loopWords, id))
5002  {
5003  // PINK = FOUND IN LOOP SIGNATURE
5004  color = Qt::magenta;
5005  }
5006  else if(_lastIds.contains(id))
5007  {
5008  // BLUE = FOUND IN LAST SIGNATURE
5009  color = Qt::blue;
5010  }
5011  else if(id<=_lastId)
5012  {
5013  // RED = ALREADY EXISTS
5014  color = Qt::red;
5015  }
5016  else if(refWords.count(id) > 1)
5017  {
5018  // YELLOW = NEW and multiple times
5019  color = Qt::yellow;
5020  }
5021  else
5022  {
5023  // GREEN = NEW
5024  color = Qt::green;
5025  }
5026  _ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
5027  }
5028  ULOGGER_DEBUG("source time = %f s", timer.ticks());
5029 
5030  timer.start();
5031  ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
5032  QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
5033  if(loopWords.size())
5034  {
5035  _ui->imageView_loopClosure->clearFeatures();
5036  }
5037  for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
5038  {
5039  int id = iter->first;
5040  QColor color;
5041  if(id<0)
5042  {
5043  // GRAY = NOT QUANTIZED
5044  color = Qt::gray;
5045  }
5046  else if(uContains(refWords, id))
5047  {
5048  // PINK = FOUND IN LOOP SIGNATURE
5049  color = Qt::magenta;
5050  //To draw lines... get only unique correspondences
5051  if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
5052  {
5053  const cv::KeyPoint & a = refWords.find(id)->second;
5054  const cv::KeyPoint & b = iter->second;
5055  uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
5056  }
5057  }
5058  else if(id<=_lastId)
5059  {
5060  // RED = ALREADY EXISTS
5061  color = Qt::red;
5062  }
5063  else if(refWords.count(id) > 1)
5064  {
5065  // YELLOW = NEW and multiple times
5066  color = Qt::yellow;
5067  }
5068  else
5069  {
5070  // GREEN = NEW
5071  color = Qt::green;
5072  }
5073  _ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
5074  }
5075 
5076  ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
5077 
5078  if(refWords.size()>0)
5079  {
5080  if((*refWords.rbegin()).first > _lastId)
5081  {
5082  _lastId = (*refWords.rbegin()).first;
5083  }
5084  std::list<int> kpts = uKeysList(refWords);
5085 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
5086  _lastIds = QSet<int>(kpts.begin(), kpts.end());
5087 #else
5088  _lastIds = QSet<int>::fromList(QList<int>::fromStdList(kpts));
5089 #endif
5090  }
5091 
5092  // Draw lines between corresponding features...
5093  float scaleSource = _ui->imageView_source->viewScale();
5094  float scaleLoop = _ui->imageView_loopClosure->viewScale();
5095  UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
5096  // Delta in actual window pixels
5097  float sourceMarginX = (_ui->imageView_source->width() - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
5098  float sourceMarginY = (_ui->imageView_source->height() - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
5099  float loopMarginX = (_ui->imageView_loopClosure->width() - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
5100  float loopMarginY = (_ui->imageView_loopClosure->height() - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
5101 
5102  float deltaX = 0;
5103  float deltaY = 0;
5104 
5105  if(_preferencesDialog->isVerticalLayoutUsed())
5106  {
5107  deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
5108  }
5109  else
5110  {
5111  deltaX = _ui->imageView_source->width();
5112  }
5113 
5114  if(refWords.size() && loopWords.size())
5115  {
5116  _ui->imageView_source->clearLines();
5117  _ui->imageView_loopClosure->clearLines();
5118  }
5119 
5120  for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
5121  iter!=uniqueCorrespondences.end();
5122  ++iter)
5123  {
5124 
5125  _ui->imageView_source->addLine(
5126  iter->first.x,
5127  iter->first.y,
5128  (iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
5129  (iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
5130  _ui->imageView_source->getDefaultMatchingLineColor());
5131 
5132  _ui->imageView_loopClosure->addLine(
5133  (iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
5134  (iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
5135  iter->second.x,
5136  iter->second.y,
5137  _ui->imageView_loopClosure->getDefaultMatchingLineColor());
5138  }
5139  _ui->imageView_source->update();
5140  _ui->imageView_loopClosure->update();
5141 }
5142 
5143 void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
5144 {
5145  for(std::map<int, Link>::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter)
5146  {
5147  // Project in all cameras in which the landmark is visible
5148  for(size_t i=0; i<signature.sensorData().cameraModels().size() || i<signature.sensorData().stereoCameraModels().size(); ++i)
5149  {
5150  CameraModel model;
5151  if(i<signature.sensorData().cameraModels().size())
5152  {
5153  model = signature.sensorData().cameraModels()[i];
5154  }
5155  else if(i<signature.sensorData().stereoCameraModels().size())
5156  {
5157  model = signature.sensorData().stereoCameraModels()[i].left();
5158  }
5159  if(model.isValidForProjection())
5160  {
5161  Transform t = model.localTransform().inverse() * iter->second.transform();
5162  cv::Vec3d rvec, tvec;
5163  tvec.val[0] = t.x();
5164  tvec.val[1] = t.y();
5165  tvec.val[2] = t.z();
5166 
5167  // In front of the camera?
5168  if(t.z() > 0)
5169  {
5170  cv::Mat R;
5171  t.rotationMatrix().convertTo(R, CV_64F);
5172  cv::Rodrigues(R, rvec);
5173 
5174  //cv::aruco::drawAxis(image, model.K(), model.D(), rvec, tvec, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f);
5175 
5176  // project axis points
5177  std::vector< cv::Point3f > axisPoints;
5178  float length = _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f;
5179  axisPoints.push_back(cv::Point3f(0, 0, 0));
5180  axisPoints.push_back(cv::Point3f(length, 0, 0));
5181  axisPoints.push_back(cv::Point3f(0, length, 0));
5182  axisPoints.push_back(cv::Point3f(0, 0, length));
5183  std::vector< cv::Point2f > imagePoints;
5184  projectPoints(axisPoints, rvec, tvec, model.K(), model.D(), imagePoints);
5185 
5186  //offset x based on camera index
5187  bool valid = true;
5188  if(i!=0)
5189  {
5190  if(model.imageWidth() <= 0)
5191  {
5192  valid = false;
5193  UWARN("Cannot draw correctly landmark %d with provided camera model %d (missing image width)", -iter->first, (int)i);
5194  }
5195  else
5196  {
5197  for(int j=0; j<4; ++j)
5198  {
5199  imagePoints[j].x += i*model.imageWidth();
5200  }
5201  }
5202  }
5203  if(valid)
5204  {
5205  // draw axis lines
5206  cv::line(image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 3);
5207  cv::line(image, imagePoints[0], imagePoints[2], cv::Scalar(0, 255, 0), 3);
5208  cv::line(image, imagePoints[0], imagePoints[3], cv::Scalar(255, 0, 0), 3);
5209  cv::putText(image, uNumber2Str(-iter->first), imagePoints[0], cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 255), 2);
5210  }
5211  }
5212  }
5213  }
5214  }
5215 }
5216 
5217 void MainWindow::showEvent(QShowEvent* anEvent)
5218 {
5219  //if the config file doesn't exist, make the GUI obsolete
5220  this->setWindowModified(!QFile::exists(_preferencesDialog->getIniFilePath()));
5221 }
5222 
5223 void MainWindow::moveEvent(QMoveEvent* anEvent)
5224 {
5225  if(this->isVisible())
5226  {
5227  // HACK, there is a move event when the window is shown the first time.
5228  if(!_firstCall)
5229  {
5230  this->configGUIModified();
5231  }
5232  _firstCall = false;
5233  }
5234 }
5235 
5236 void MainWindow::resizeEvent(QResizeEvent* anEvent)
5237 {
5238  if(this->isVisible())
5239  {
5240  this->configGUIModified();
5241  }
5242 }
5243 
5244 void MainWindow::keyPressEvent(QKeyEvent *event)
5245 {
5246  //catch ctrl-s to save settings
5247  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
5248  {
5249  this->saveConfigGUI();
5250  }
5251 }
5252 
5253 bool MainWindow::eventFilter(QObject *obj, QEvent *event)
5254 {
5255  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
5256  {
5257  this->setWindowModified(true);
5258  }
5259  else if(event->type() == QEvent::FileOpen )
5260  {
5261  openDatabase(((QFileOpenEvent*)event)->file());
5262  }
5263  return QWidget::eventFilter(obj, event);
5264 }
5265 
5266 void MainWindow::updateSelectSourceMenu()
5267 {
5268  _ui->actionMore_options->setChecked(
5269  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
5270  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice ||
5271  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
5272  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
5273  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
5274  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
5275  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages
5276  );
5277 
5278  _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
5279  _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
5280  _ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
5281  _ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
5282  _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
5283  _ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
5284  _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
5285  _ui->actionOpenNI2_orbbec->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
5286  _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
5287  _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
5288  _ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
5289  _ui->actionKinect_for_Azure->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4A);
5290  _ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
5291  _ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
5292  _ui->actionRealSense2_SR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
5293  _ui->actionRealSense2_D400->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
5294  _ui->actionRealSense2_L515->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
5295  _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
5296  _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
5297  _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
5298  _ui->actionZed_Open_Capture->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZedOC);
5299  _ui->actionStereoTara->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoTara);
5300  _ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
5301  _ui->actionRealSense2_T265->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoRealSense2);
5302  _ui->actionMYNT_EYE_S_SDK->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoMyntEye);
5303  _ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
5304  _ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
5305  _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
5306  _ui->actionXvisio_SeerSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcSeerSense);
5307  _ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16);
5308 }
5309 
5310 void MainWindow::changeImgRateSetting()
5311 {
5312  Q_EMIT imgRateChanged(_ui->doubleSpinBox_stats_imgRate->value());
5313 }
5314 
5315 void MainWindow::changeDetectionRateSetting()
5316 {
5317  Q_EMIT detectionRateChanged(_ui->doubleSpinBox_stats_detectionRate->value());
5318 }
5319 
5320 void MainWindow::changeTimeLimitSetting()
5321 {
5322  Q_EMIT timeLimitChanged((float)_ui->doubleSpinBox_stats_timeLimit->value());
5323 }
5324 
5325 void MainWindow::changeMappingMode()
5326 {
5327  Q_EMIT mappingModeChanged(_ui->actionSLAM_mode->isChecked());
5328 }
5329 
5330 QString MainWindow::captureScreen(bool cacheInRAM, bool png)
5331 {
5332  QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (png?".png":".jpg"));
5333  _ui->statusbar->clearMessage();
5334  QPixmap figure = this->grab();
5335 
5336  QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
5337  QString msg;
5338  if(cacheInRAM)
5339  {
5340  msg = tr("Screen captured \"%1\"").arg(name);
5341  QByteArray bytes;
5342  QBuffer buffer(&bytes);
5343  buffer.open(QIODevice::WriteOnly);
5344  figure.save(&buffer, png?"PNG":"JPEG");
5345  _autoScreenCaptureCachedImages.insert(name, bytes);
5346  }
5347  else
5348  {
5349  QDir dir;
5350  if(!dir.exists(targetDir))
5351  {
5352  dir.mkdir(targetDir);
5353  }
5354  targetDir += QDir::separator();
5355  targetDir += "Main_window";
5356  if(!dir.exists(targetDir))
5357  {
5358  dir.mkdir(targetDir);
5359  }
5360  targetDir += QDir::separator();
5361 
5362  figure.save(targetDir + name);
5363  msg = tr("Screen captured \"%1\"").arg(targetDir + name);
5364  }
5365  _ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
5366  _ui->widget_console->appendMsg(msg);
5367 
5368  return targetDir + name;
5369 }
5370 
5371 void MainWindow::beep()
5372 {
5373  QApplication::beep();
5374 }
5375 
5376 void MainWindow::cancelProgress()
5377 {
5378  _progressCanceled = true;
5379  _progressDialog->appendText(tr("Canceled!"));
5380 }
5381 
5382 void MainWindow::configGUIModified()
5383 {
5384  this->setWindowModified(true);
5385 }
5386 
5387 void MainWindow::updateParameters(const ParametersMap & parameters)
5388 {
5389  if(parameters.size())
5390  {
5391  for(ParametersMap::const_iterator iter= parameters.begin(); iter!=parameters.end(); ++iter)
5392  {
5393  QString msg = tr("Parameter update \"%1\"=\"%2\"")
5394  .arg(iter->first.c_str())
5395  .arg(iter->second.c_str());
5396  _ui->widget_console->appendMsg(msg);
5397  UWARN(msg.toStdString().c_str());
5398  }
5399  QMessageBox::StandardButton button = QMessageBox::question(this,
5400  tr("Parameters"),
5401  tr("Some parameters have been set on command line, do you "
5402  "want to set all other RTAB-Map's parameters to default?"),
5403  QMessageBox::Yes | QMessageBox::No,
5404  QMessageBox::No);
5405  _preferencesDialog->updateParameters(parameters, button==QMessageBox::Yes);
5406  }
5407 }
5408 
5409 //ACTIONS
5410 void MainWindow::saveConfigGUI()
5411 {
5412  _savedMaximized = this->isMaximized();
5413  _preferencesDialog->saveMainWindowState(this);
5414  _preferencesDialog->saveWindowGeometry(_preferencesDialog);
5415  _preferencesDialog->saveWindowGeometry(_aboutDialog);
5416  _preferencesDialog->saveWidgetState(_cloudViewer);
5417  _preferencesDialog->saveWidgetState(_ui->imageView_source);
5418  _preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
5419  _preferencesDialog->saveWidgetState(_ui->imageView_odometry);
5420  _preferencesDialog->saveWidgetState(_exportCloudsDialog);
5421  _preferencesDialog->saveWidgetState(_exportBundlerDialog);
5422  _preferencesDialog->saveWidgetState(_postProcessingDialog);
5423  _preferencesDialog->saveWidgetState(_depthCalibrationDialog);
5424  _preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
5425  _preferencesDialog->saveWidgetState(_multiSessionLocWidget->getImageView());
5426  _preferencesDialog->saveSettings();
5427  this->saveFigures();
5428  this->setWindowModified(false);
5429 }
5430 
5431 void MainWindow::newDatabase()
5432 {
5433  if(_state != MainWindow::kIdle)
5434  {
5435  UERROR("This method can be called only in IDLE state.");
5436  return;
5437  }
5438  _openedDatabasePath.clear();
5439  _newDatabasePath.clear();
5440  _newDatabasePathOutput.clear();
5441  _databaseUpdated = false;
5442  _cloudViewer->removeLine("map_to_odom");
5443  _cloudViewer->removeLine("odom_to_base_link");
5444  _cloudViewer->removeCoordinate("odom_frame");
5445  _cloudViewer->removeCoordinate("map_frame");
5446  _cloudViewer->removeText("map_frame_label");
5447  _cloudViewer->removeText("odom_frame_label");
5448  ULOGGER_DEBUG("");
5449  this->clearTheCache();
5450  std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString();
5451  if(QFile::exists(databasePath.c_str()))
5452  {
5453  int r = QMessageBox::question(this,
5454  tr("Creating temporary database"),
5455  tr("Cannot create a new database because the temporary database \"%1\" already exists. "
5456  "There may be another instance of RTAB-Map running with the same Working Directory or "
5457  "the last time RTAB-Map was not closed correctly. "
5458  "Do you want to recover the database (click Ignore to delete it and create a new one)?").arg(databasePath.c_str()),
5459  QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::No);
5460 
5461  if(r == QMessageBox::Ignore)
5462  {
5463  if(QFile::remove(databasePath.c_str()))
5464  {
5465  UINFO("Deleted temporary database \"%s\".", databasePath.c_str());
5466  }
5467  else
5468  {
5469  UERROR("Temporary database \"%s\" could not be deleted!", databasePath.c_str());
5470  return;
5471  }
5472  }
5473  else if(r == QMessageBox::Yes)
5474  {
5475  std::string errorMsg;
5476  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
5477  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
5478  progressDialog->setMaximumSteps(100);
5479  progressDialog->show();
5480  progressDialog->setCancelButtonVisible(true);
5481  RecoveryState state(progressDialog);
5482  _recovering = true;
5483  if(databaseRecovery(databasePath, false, &errorMsg, &state))
5484  {
5485  _recovering = false;
5486  progressDialog->setValue(progressDialog->maximumSteps());
5487  QString newPath = QFileDialog::getSaveFileName(this, tr("Save recovered database"), _preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("recovered.db"), tr("RTAB-Map database files (*.db)"));
5488  if(newPath.isEmpty())
5489  {
5490  return;
5491  }
5492  if(QFileInfo(newPath).suffix() == "")
5493  {
5494  newPath += ".db";
5495  }
5496  if(QFile::exists(newPath))
5497  {
5498  QFile::remove(newPath);
5499  }
5500  QFile::rename(databasePath.c_str(), newPath);
5501  return;
5502  }
5503  else
5504  {
5505  _recovering = false;
5506  progressDialog->setValue(progressDialog->maximumSteps());
5507  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
5508  return;
5509  }
5510  }
5511  else
5512  {
5513  return;
5514  }
5515  }
5516  _newDatabasePath = databasePath.c_str();
5517  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
5518  applyPrefSettings(_preferencesDialog->getAllParameters(), false);
5519 }
5520 
5521 void MainWindow::openDatabase()
5522 {
5523  QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _defaultOpenDatabasePath.isEmpty()?_preferencesDialog->getWorkingDirectory():_defaultOpenDatabasePath, tr("RTAB-Map database files (*.db)"));
5524  if(!path.isEmpty())
5525  {
5526  this->openDatabase(path);
5527  }
5528 }
5529 
5530 void MainWindow::openDatabase(const QString & path, const ParametersMap & overridedParameters)
5531 {
5532  if(_state != MainWindow::kIdle)
5533  {
5534  UERROR("Database can only be opened in IDLE state.");
5535  return;
5536  }
5537 
5538  std::string value = path.toStdString();
5539  if(UFile::exists(value) &&
5540  UFile::getExtension(value).compare("db") == 0)
5541  {
5542  _openedDatabasePath.clear();
5543  _newDatabasePath.clear();
5544  _newDatabasePathOutput.clear();
5545  _databaseUpdated = false;
5546 
5547  this->clearTheCache();
5548  _openedDatabasePath = path;
5549  _defaultOpenDatabasePath = path;
5550 
5551  // look if there are saved parameters
5552  DBDriver * driver = DBDriver::create();
5553  if(driver->openConnection(value, false))
5554  {
5555  ParametersMap parameters = driver->getLastParameters();
5556  driver->closeConnection(false);
5557  delete driver;
5558 
5559  if(parameters.size())
5560  {
5561  //backward compatibility with databases not saving all parameters, use default for not saved ones
5562  for(ParametersMap::const_iterator iter=Parameters::getDefaultParameters().begin(); iter!=Parameters::getDefaultParameters().end(); ++iter)
5563  {
5564  parameters.insert(*iter);
5565  }
5566 
5567  uInsert(parameters, overridedParameters);
5568 
5569  ParametersMap currentParameters = _preferencesDialog->getAllParameters();
5570  ParametersMap differentParameters;
5571  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
5572  {
5573  ParametersMap::iterator jter = currentParameters.find(iter->first);
5574  if(jter!=currentParameters.end() &&
5575  iter->second.compare(jter->second) != 0 &&
5576  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
5577  {
5578  bool different = true;
5579  if(Parameters::getType(iter->first).compare("double") ==0 ||
5580  Parameters::getType(iter->first).compare("float") == 0)
5581  {
5582  if(uStr2Double(iter->second) == uStr2Double(jter->second))
5583  {
5584  different = false;
5585  }
5586  }
5587  else if(Parameters::getType(iter->first).compare("bool") == 0)
5588  {
5589  if(uStr2Bool(iter->second) == uStr2Bool(jter->second))
5590  {
5591  different = false;
5592  }
5593  }
5594  if(different)
5595  {
5596  differentParameters.insert(*iter);
5597  QString msg = tr("Parameter \"%1\": %2=\"%3\" Preferences=\"%4\"")
5598  .arg(iter->first.c_str())
5599  .arg(overridedParameters.find(iter->first) != overridedParameters.end()?"Arguments":"Database")
5600  .arg(iter->second.c_str())
5601  .arg(jter->second.c_str());
5602  _ui->widget_console->appendMsg(msg);
5603  UWARN(msg.toStdString().c_str());
5604  }
5605  }
5606  }
5607 
5608  if(differentParameters.size())
5609  {
5610  int r = QMessageBox::question(this,
5611  tr("Update parameters..."),
5612  tr("The %1 using %2 different parameter(s) than "
5613  "those currently set in Preferences. Do you want "
5614  "to use those parameters?")
5615  .arg(overridedParameters.empty()?tr("database is"):tr("database and input arguments are"))
5616  .arg(differentParameters.size()),
5617  QMessageBox::Yes | QMessageBox::No,
5618  QMessageBox::Yes);
5619  if(r == QMessageBox::Yes)
5620  {
5621  _preferencesDialog->updateParameters(differentParameters);
5622  }
5623  }
5624  }
5625  }
5626 
5627  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, value, 0, _preferencesDialog->getAllParameters()));
5628  applyPrefSettings(_preferencesDialog->getAllParameters(), false);
5629  }
5630  else
5631  {
5632  UERROR("File \"%s\" not valid.", value.c_str());
5633  }
5634 }
5635 
5636 bool MainWindow::closeDatabase()
5637 {
5638  if(_state != MainWindow::kInitialized)
5639  {
5640  UERROR("This method can be called only in INITIALIZED state.");
5641  return false;
5642  }
5643 
5644  _newDatabasePathOutput.clear();
5645  if(!_newDatabasePath.isEmpty() && _databaseUpdated)
5646  {
5647  QMessageBox::Button b = QMessageBox::question(this,
5648  tr("Save database"),
5649  tr("Save the new database?"),
5650  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard,
5651  QMessageBox::Save);
5652 
5653  if(b == QMessageBox::Save)
5654  {
5655  // Temp database used, automatically backup with unique name (timestamp)
5656  QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
5657  QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
5658 
5659  newPath = QFileDialog::getSaveFileName(this, tr("Save database"), newPath, tr("RTAB-Map database files (*.db)"));
5660  if(newPath.isEmpty())
5661  {
5662  return false;
5663  }
5664 
5665  if(QFileInfo(newPath).suffix() == "")
5666  {
5667  newPath += ".db";
5668  }
5669 
5670  _newDatabasePathOutput = newPath;
5671  }
5672  else if(b != QMessageBox::Discard)
5673  {
5674  return false;
5675  }
5676  }
5677 
5678  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_openedDatabasePath.isEmpty() || !_newDatabasePathOutput.isEmpty()));
5679  return true;
5680 }
5681 
5682 void MainWindow::editDatabase()
5683 {
5684  if(_state != MainWindow::kIdle)
5685  {
5686  UERROR("This method can be called only in IDLE state.");
5687  return;
5688  }
5689  QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
5690  if(!path.isEmpty())
5691  {
5692  {
5693  // copy database settings to tmp ini file
5694  QSettings dbSettingsIn(_preferencesDialog->getIniFilePath(), QSettings::IniFormat);
5695  QSettings dbSettingsOut(_preferencesDialog->getTmpIniFilePath(), QSettings::IniFormat);
5696  dbSettingsIn.beginGroup("DatabaseViewer");
5697  dbSettingsOut.beginGroup("DatabaseViewer");
5698  QStringList keys = dbSettingsIn.childKeys();
5699  for(QStringList::iterator iter = keys.begin(); iter!=keys.end(); ++iter)
5700  {
5701  dbSettingsOut.setValue(*iter, dbSettingsIn.value(*iter));
5702  }
5703  dbSettingsIn.endGroup();
5704  dbSettingsOut.endGroup();
5705  }
5706 
5707  DatabaseViewer * viewer = new DatabaseViewer(_preferencesDialog->getTmpIniFilePath(), this);
5708  viewer->setWindowModality(Qt::WindowModal);
5709  viewer->setAttribute(Qt::WA_DeleteOnClose, true);
5710  viewer->showCloseButton();
5711 
5712  if(viewer->isSavedMaximized())
5713  {
5714  viewer->showMaximized();
5715  }
5716  else
5717  {
5718  viewer->show();
5719  }
5720 
5721  viewer->openDatabase(path);
5722  }
5723 }
5724 
5725 void MainWindow::startDetection()
5726 {
5727  UDEBUG("");
5728  ParametersMap parameters = _preferencesDialog->getAllParameters();
5729  uInsert(parameters, this->getCustomParameters());
5730 
5731  // verify source with input rates
5732  if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
5733  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
5734  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
5735  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
5736  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
5737  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
5738  {
5739  float inputRate = _preferencesDialog->getGeneralInputRate();
5740  float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
5741  int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
5742  if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) &&
5743  (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed()))
5744  {
5745  int button = QMessageBox::question(this,
5746  tr("Incompatible frame rates!"),
5747  tr("\"Source/Input rate\" (%1 Hz) is equal to/higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the "
5748  "source input is a directory of images/video/database, some images may be "
5749  "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over "
5750  "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to "
5751  "start the detection anyway?").arg(inputRate).arg(detectionRate),
5752  QMessageBox::Yes | QMessageBox::No);
5753  if(button == QMessageBox::No)
5754  {
5755  return;
5756  }
5757  }
5758  if(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed())
5759  {
5760  if(bufferingSize != 0)
5761  {
5762  int button = QMessageBox::question(this,
5763  tr("Some images may be skipped!"),
5764  tr("\"RTAB-Map/Images buffer size\" is not infinite (size=%1). As the "
5765  "source input is a directory of images/video/database, some images may be "
5766  "skipped by the detector if the \"Source/Input rate\" (which is %2 Hz) is higher than the "
5767  "rate at which RTAB-Map can process the images. You may want to set the "
5768  "\"RTAB-Map/Images buffer size\" to 0 (infinite) to guaranty that all "
5769  "images are processed. Would you want to start the detection "
5770  "anyway?").arg(bufferingSize).arg(inputRate),
5771  QMessageBox::Yes | QMessageBox::No);
5772  if(button == QMessageBox::No)
5773  {
5774  return;
5775  }
5776  }
5777  else if(inputRate == 0)
5778  {
5779  int button = QMessageBox::question(this,
5780  tr("Large number of images may be buffered!"),
5781  tr("\"RTAB-Map/Images buffer size\" is infinite. As the "
5782  "source input is a directory of images/video/database and "
5783  "that \"Source/Input rate\" is infinite too, a lot of images "
5784  "could be buffered at the same time (e.g., reading all images "
5785  "of a directory at once). This could make the GUI not responsive. "
5786  "You may want to set \"Source/Input rate\" at the rate at "
5787  "which the images have been recorded. "
5788  "Would you want to start the detection "
5789  "anyway?").arg(bufferingSize).arg(inputRate),
5790  QMessageBox::Yes | QMessageBox::No);
5791  if(button == QMessageBox::No)
5792  {
5793  return;
5794  }
5795  }
5796  }
5797  }
5798 
5799  UDEBUG("");
5800  Q_EMIT stateChanged(kStartingDetection);
5801 
5802  if(_sensorCapture != 0)
5803  {
5804  QMessageBox::warning(this,
5805  tr("RTAB-Map"),
5806  tr("A camera is running, stop it first."));
5807  UWARN("_sensorCapture is not null... it must be stopped first");
5808  Q_EMIT stateChanged(kInitialized);
5809  return;
5810  }
5811 
5812  // Adjust pre-requirements
5813  if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef &&
5814  _preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcUndef)
5815  {
5816  QMessageBox::warning(this,
5817  tr("RTAB-Map"),
5818  tr("No sources are selected. See Preferences->Source panel."));
5819  UWARN("No sources are selected. See Preferences->Source panel.");
5820  Q_EMIT stateChanged(kInitialized);
5821  return;
5822  }
5823 
5824 
5825  double poseTimeOffset = 0.0;
5826  float scaleFactor = 0.0f;
5827  double waitTime = 0.1;
5828  Transform extrinsics;
5829  SensorCapture * odomSensor = 0;
5830  Camera * camera = 0;
5831  Lidar * lidar = 0;
5832 
5833  if(_preferencesDialog->getLidarSourceDriver() != PreferencesDialog::kSrcUndef)
5834  {
5835  lidar = _preferencesDialog->createLidar();
5836  if(!lidar)
5837  {
5838  Q_EMIT stateChanged(kInitialized);
5839  return;
5840  }
5841  }
5842 
5843  if(!lidar || _preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcUndef)
5844  {
5845  camera = _preferencesDialog->createCamera();
5846  if(!camera)
5847  {
5848  delete lidar;
5849  Q_EMIT stateChanged(kInitialized);
5850  return;
5851  }
5852  }
5853 
5854  if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase &&
5855  camera && camera->odomProvided())
5856  {
5857  odomSensor = camera;
5858  }
5859  else if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef)
5860  {
5861  if(camera == 0 ||
5862  (_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() &&
5863  !(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 &&
5864  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2)))
5865  {
5866  UINFO("Create Odom Sensor %d (camera = %d)",
5867  _preferencesDialog->getOdomSourceDriver(),
5868  _preferencesDialog->getSourceDriver());
5869  odomSensor = _preferencesDialog->createOdomSensor(extrinsics, poseTimeOffset, scaleFactor, waitTime);
5870  if(!odomSensor)
5871  {
5872  delete camera;
5873  delete lidar;
5874  Q_EMIT stateChanged(kInitialized);
5875  return;
5876  }
5877  }
5878  else if(camera->odomProvided())
5879  {
5880  UINFO("The camera is also the odometry sensor (camera=%d odom=%d).",
5881  _preferencesDialog->getSourceDriver(),
5882  _preferencesDialog->getOdomSourceDriver());
5883  odomSensor = camera;
5884  }
5885  }
5886 
5887  _sensorCapture = new SensorCaptureThread(lidar, camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, waitTime, parameters);
5888 
5889  _sensorCapture->setOdomAsGroundTruth(_preferencesDialog->isOdomSensorAsGt());
5890  _sensorCapture->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
5891  _sensorCapture->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
5892  _sensorCapture->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
5893  _sensorCapture->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod());
5894  if(_preferencesDialog->isSourceFeatureDetection())
5895  {
5896  _sensorCapture->enableFeatureDetection(parameters);
5897  }
5898  _sensorCapture->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
5899  _sensorCapture->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation());
5900  _sensorCapture->setScanParameters(
5901  _preferencesDialog->isSourceScanFromDepth(),
5902  _preferencesDialog->getSourceScanDownsampleStep(),
5903  _preferencesDialog->getSourceScanRangeMin(),
5904  _preferencesDialog->getSourceScanRangeMax(),
5905  _preferencesDialog->getSourceScanVoxelSize(),
5906  _preferencesDialog->getSourceScanNormalsK(),
5907  _preferencesDialog->getSourceScanNormalsRadius(),
5908  (float)_preferencesDialog->getSourceScanForceGroundNormalsUp(),
5909  _preferencesDialog->isSourceScanDeskewing());
5910  if(_preferencesDialog->getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
5911  {
5912  _sensorCapture->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion());
5913  }
5914  if(_preferencesDialog->isDepthFilteringAvailable())
5915  {
5916  if(_preferencesDialog->isBilateralFiltering())
5917  {
5918  _sensorCapture->enableBilateralFiltering(
5919  _preferencesDialog->getBilateralSigmaS(),
5920  _preferencesDialog->getBilateralSigmaR());
5921  }
5922  _sensorCapture->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString());
5923  }
5924 
5925  //Create odometry thread if rgbd slam
5926  if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
5927  {
5928  // Require calibrated camera
5929  if(camera && !camera->isCalibrated())
5930  {
5931  UWARN("Camera is not calibrated!");
5932  Q_EMIT stateChanged(kInitialized);
5933  delete _sensorCapture;
5934  _sensorCapture = 0;
5935 
5936  int button = QMessageBox::question(this,
5937  tr("Camera is not calibrated!"),
5938  tr("RTAB-Map in metric SLAM mode cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
5939  QMessageBox::Yes | QMessageBox::No);
5940  if(button == QMessageBox::Yes)
5941  {
5942  QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
5943  }
5944  return;
5945  }
5946  else
5947  {
5948  if(_odomThread)
5949  {
5950  UERROR("OdomThread must be already deleted here?!");
5951  delete _odomThread;
5952  _odomThread = 0;
5953  }
5954 
5955  if(_imuThread)
5956  {
5957  UERROR("ImuThread must be already deleted here?!");
5958  delete _imuThread;
5959  _imuThread = 0;
5960  }
5961 
5962  if(!_sensorCapture->odomProvided() && !_preferencesDialog->isOdomDisabled())
5963  {
5964  ParametersMap odomParameters = parameters;
5965  if(_preferencesDialog->getOdomRegistrationApproach() < 3)
5966  {
5967  uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
5968  }
5969  odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
5970  int odomStrategy = Parameters::defaultOdomStrategy();
5971  Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
5972  double gravitySigma = _preferencesDialog->getOdomF2MGravitySigma();
5973  UDEBUG("Odom gravitySigma=%f", gravitySigma);
5974  if(gravitySigma >= 0.0)
5975  {
5976  uInsert(odomParameters, ParametersPair(Parameters::kOptimizerGravitySigma(), uNumber2Str(gravitySigma)));
5977  }
5978  if(odomStrategy != 1)
5979  {
5980  // Only Frame To Frame supports all VisCorType
5981  odomParameters.erase(Parameters::kVisCorType());
5982  }
5983  _imuThread = 0;
5984  if((_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
5985  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
5986  _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
5987  !_preferencesDialog->getIMUPath().isEmpty())
5988  {
5989  _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
5990  if(_preferencesDialog->getIMUFilteringStrategy()>0)
5991  {
5992  _imuThread->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion());
5993  }
5994  if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
5995  {
5996  QMessageBox::warning(this, tr("Source IMU Path"),
5997  tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
5998  delete _sensorCapture;
5999  _sensorCapture = 0;
6000  delete _imuThread;
6001  _imuThread = 0;
6002  return;
6003  }
6004  }
6005  Odometry * odom = Odometry::create(odomParameters);
6006  _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
6007 
6008  UEventsManager::addHandler(_odomThread);
6009  UEventsManager::createPipe(_sensorCapture, _odomThread, "SensorEvent");
6010  UEventsManager::createPipe(_sensorCapture, this, "SensorEvent");
6011  if(_imuThread)
6012  {
6013  UEventsManager::createPipe(_imuThread, _odomThread, "IMUEvent");
6014  }
6015  _odomThread->start();
6016  }
6017  }
6018  }
6019 
6020  if(_dataRecorder && _sensorCapture && _odomThread)
6021  {
6022  UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent");
6023  }
6024 
6025  _lastOdomPose.setNull();
6026  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer)); // clean sensors buffer
6027  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap)); // Trigger a new map
6028 
6029  if(_odomThread)
6030  {
6031  _ui->actionReset_Odometry->setEnabled(true);
6032  }
6033 
6034  if(!_preferencesDialog->isStatisticsPublished())
6035  {
6036  QMessageBox::information(this,
6037  tr("Information"),
6038  tr("Note that publishing statistics is disabled, "
6039  "progress will not be shown in the GUI."));
6040  }
6041 
6042  // Override map resolution for UI
6043  if(_preferencesDialog->getGridUIResolution()>0)
6044  {
6045  uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution())));
6046  }
6047 
6048  _cachedLocalMaps.clear();
6049 
6050  delete _occupancyGrid;
6051  _occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters);
6052 
6053 #ifdef RTABMAP_OCTOMAP
6054  delete _octomap;
6055  _octomap = new OctoMap(&_cachedLocalMaps, parameters);
6056 #endif
6057 
6058 #ifdef RTABMAP_GRIDMAP
6059  delete _elevationMap;
6060  _elevationMap = new GridMap(&_cachedLocalMaps, parameters);
6061 #endif
6062 
6063  // clear odometry visual stuff
6064  _cloudViewer->removeCloud("cloudOdom");
6065  _cloudViewer->removeCloud("scanOdom");
6066  _cloudViewer->removeCloud("scanMapOdom");
6067  _cloudViewer->removeCloud("featuresOdom");
6068  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
6069 
6070  Q_EMIT stateChanged(kDetecting);
6071 }
6072 
6073 // Could not be in the main thread here! (see handleEvents())
6074 void MainWindow::pauseDetection()
6075 {
6076  if(_sensorCapture)
6077  {
6078  if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
6079  {
6080  // On Ctrl-click, start the camera and pause it automatically
6081  Q_EMIT stateChanged(kPaused);
6082  if(_preferencesDialog->getGeneralInputRate())
6083  {
6084  QTimer::singleShot(500.0/_preferencesDialog->getGeneralInputRate(), this, SLOT(pauseDetection()));
6085  }
6086  else
6087  {
6088  Q_EMIT stateChanged(kPaused);
6089  }
6090  }
6091  else
6092  {
6093  Q_EMIT stateChanged(kPaused);
6094  }
6095  }
6096  else if(_state == kMonitoring)
6097  {
6098  UINFO("Sending pause event!");
6099  Q_EMIT stateChanged(kMonitoringPaused);
6100  }
6101  else if(_state == kMonitoringPaused)
6102  {
6103  UINFO("Sending unpause event!");
6104  Q_EMIT stateChanged(kMonitoring);
6105  }
6106 }
6107 
6108 void MainWindow::stopDetection()
6109 {
6110  if(!_sensorCapture && !_odomThread)
6111  {
6112  return;
6113  }
6114 
6115  if(_state == kDetecting &&
6116  (_sensorCapture && _sensorCapture->isRunning()) )
6117  {
6118  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);
6119 
6120  if(button != QMessageBox::Yes)
6121  {
6122  return;
6123  }
6124  }
6125 
6126  ULOGGER_DEBUG("");
6127  // kill the processes
6128  if(_imuThread)
6129  {
6130  _imuThread->join(true);
6131  }
6132 
6133  if(_sensorCapture)
6134  {
6135  _sensorCapture->join(true);
6136  }
6137 
6138  if(_odomThread)
6139  {
6140  _ui->actionReset_Odometry->setEnabled(false);
6141  _odomThread->kill();
6142  }
6143 
6144  // delete the processes
6145  if(_imuThread)
6146  {
6147  delete _imuThread;
6148  _imuThread = 0;
6149  }
6150  if(_sensorCapture)
6151  {
6152  delete _sensorCapture;
6153  _sensorCapture = 0;
6154  }
6155  if(_odomThread)
6156  {
6157  delete _odomThread;
6158  _odomThread = 0;
6159  }
6160 
6161  if(_dataRecorder)
6162  {
6163  delete _dataRecorder;
6164  _dataRecorder = 0;
6165  }
6166 
6167  Q_EMIT stateChanged(kInitialized);
6168 }
6169 
6170 void MainWindow::notifyNoMoreImages()
6171 {
6172  QMessageBox::information(this,
6173  tr("No more images..."),
6174  tr("The camera has reached the end of the stream."));
6175 }
6176 
6177 void MainWindow::printLoopClosureIds()
6178 {
6179  _ui->dockWidget_console->show();
6180  QString msgRef;
6181  QString msgLoop;
6182  for(int i = 0; i<_refIds.size(); ++i)
6183  {
6184  msgRef.append(QString::number(_refIds[i]));
6185  msgLoop.append(QString::number(_loopClosureIds[i]));
6186  if(i < _refIds.size() - 1)
6187  {
6188  msgRef.append(" ");
6189  msgLoop.append(" ");
6190  }
6191  }
6192  _ui->widget_console->appendMsg(QString("IDs = [%1];").arg(msgRef));
6193  _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
6194 }
6195 
6196 void MainWindow::generateGraphDOT()
6197 {
6198  if(_graphSavingFileName.isEmpty())
6199  {
6200  _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
6201  }
6202 
6203  bool ok;
6204  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
6205  if(ok)
6206  {
6207  int margin = 0;
6208  if(id > 0)
6209  {
6210  margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
6211  }
6212 
6213  if(ok)
6214  {
6215  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
6216  if(!path.isEmpty())
6217  {
6218  _graphSavingFileName = path;
6219  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
6220 
6221  _ui->dockWidget_console->show();
6222  _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName));
6223  }
6224  }
6225  }
6226 }
6227 
6228 void MainWindow::exportPosesRaw()
6229 {
6230  exportPoses(0);
6231 }
6232 void MainWindow::exportPosesRGBDSLAMMotionCapture()
6233 {
6234  exportPoses(1);
6235 }
6236 void MainWindow::exportPosesRGBDSLAM()
6237 {
6238  exportPoses(10);
6239 }
6240 void MainWindow::exportPosesRGBDSLAMID()
6241 {
6242  exportPoses(11);
6243 }
6244 void MainWindow::exportPosesKITTI()
6245 {
6246  exportPoses(2);
6247 }
6248 void MainWindow::exportPosesTORO()
6249 {
6250  exportPoses(3);
6251 }
6252 void MainWindow::exportPosesG2O()
6253 {
6254  exportPoses(4);
6255 }
6256 
6257 void MainWindow::exportPoses(int format)
6258 {
6259  if(_currentPosesMap.size())
6260  {
6261  std::map<int, Transform> localTransforms;
6262  QStringList items;
6263  items.push_back("Robot (base frame)");
6264  items.push_back("Camera");
6265  items.push_back("Scan");
6266  bool ok;
6267  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false, &ok);
6268  if(!ok || item.isEmpty())
6269  {
6270  return;
6271  }
6272  if(item.compare("Robot (base frame)") != 0)
6273  {
6274  bool cameraFrame = item.compare("Camera") == 0;
6275  _exportPosesFrame = cameraFrame?1:2;
6276  for(std::map<int, Transform>::iterator iter=_currentPosesMap.lower_bound(1); iter!=_currentPosesMap.end(); ++iter)
6277  {
6278  if(_cachedSignatures.contains(iter->first))
6279  {
6280  Transform localTransform;
6281  if(cameraFrame)
6282  {
6283  if(_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
6284  !_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull())
6285  {
6286  localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
6287  }
6288  else if(_cachedSignatures[iter->first].sensorData().stereoCameraModels().size() == 1 &&
6289  !_cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].localTransform().isNull())
6290  {
6291  localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].localTransform();
6292  }
6293  else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1 ||
6294  _cachedSignatures[iter->first].sensorData().stereoCameraModels().size()>1)
6295  {
6296  UWARN("Multi-camera is not supported (node %d)", iter->first);
6297  }
6298  else
6299  {
6300  UWARN("Missing calibration for node %d", iter->first);
6301  }
6302  }
6303  else
6304  {
6305  if(!_cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform().isNull())
6306  {
6307  localTransform = _cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform();
6308  }
6309  else if(!_cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform().isNull())
6310  {
6311  localTransform = _cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform();
6312  }
6313  else
6314  {
6315  UWARN("Missing scan info for node %d", iter->first);
6316  }
6317  }
6318  if(!localTransform.isNull())
6319  {
6320  localTransforms.insert(std::make_pair(iter->first, localTransform));
6321  }
6322  }
6323  else
6324  {
6325  UWARN("Did not find node %d in cache", iter->first);
6326  }
6327  }
6328  if(localTransforms.empty())
6329  {
6330  QMessageBox::warning(this,
6331  tr("Export Poses"),
6332  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
6333  }
6334  }
6335  else
6336  {
6337  _exportPosesFrame = 0;
6338  }
6339 
6340  std::map<int, Transform> poses;
6341  std::multimap<int, Link> links;
6342  if(localTransforms.empty())
6343  {
6344  poses = std::map<int, Transform>(_currentPosesMap.begin(), _currentPosesMap.end());
6345  links = std::multimap<int, Link>(_currentLinksMap.begin(), _currentLinksMap.end());
6346  }
6347  else
6348  {
6349  //adjust poses and links
6350  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
6351  {
6352  poses.insert(std::make_pair(iter->first, _currentPosesMap.at(iter->first) * iter->second));
6353  }
6354  for(std::multimap<int, Link>::iterator iter=_currentLinksMap.lower_bound(1); iter!=_currentLinksMap.end(); ++iter)
6355  {
6356  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
6357  {
6358  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
6359  int from = iter->second.from();
6360  int to = iter->second.to();
6361  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
6362  }
6363  }
6364  }
6365 
6366  if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
6367  {
6368  UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
6369  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
6370  {
6371  poses.erase(iter++);
6372  }
6373  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
6374  {
6375  if(iter->second.from() < 0 || iter->second.to() < 0)
6376  {
6377  links.erase(iter++);
6378  }
6379  else
6380  {
6381  ++iter;
6382  }
6383  }
6384  }
6385 
6386  std::map<int, double> stamps;
6387  if(format == 1 || format == 10 || format == 11)
6388  {
6389  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
6390  {
6391  if(iter->first < 0 && format == 11) // in case of landmarks
6392  {
6393  stamps.insert(std::make_pair(iter->first, 0));
6394  }
6395  else if(_cachedSignatures.contains(iter->first))
6396  {
6397  stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
6398  }
6399  }
6400  if(stamps.size()!=poses.size())
6401  {
6402  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.")
6403  .arg(poses.size()).arg(stamps.size()));
6404  return;
6405  }
6406  }
6407 
6408  if(_exportPosesFileName[format].isEmpty())
6409  {
6410  _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
6411  }
6412 
6413  QString path = QFileDialog::getSaveFileName(
6414  this,
6415  tr("Save File"),
6416  _exportPosesFileName[format],
6417  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
6418 
6419  if(!path.isEmpty())
6420  {
6421  if(QFileInfo(path).suffix() == "")
6422  {
6423  if(format == 3)
6424  {
6425  path += ".graph";
6426  }
6427  else if(format==4)
6428  {
6429  path += ".g2o";
6430  }
6431  else
6432  {
6433  path += ".txt";
6434  }
6435  }
6436 
6437  _exportPosesFileName[format] = path;
6438  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps, _preferencesDialog->getAllParameters());
6439 
6440  if(saved)
6441  {
6442  QMessageBox::information(this,
6443  tr("Export poses..."),
6444  tr("%1 saved to \"%2\".")
6445  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
6446  .arg(_exportPosesFileName[format]));
6447  }
6448  else
6449  {
6450  QMessageBox::information(this,
6451  tr("Export poses..."),
6452  tr("Failed to save %1 to \"%2\"!")
6453  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
6454  .arg(_exportPosesFileName[format]));
6455  }
6456  }
6457  }
6458 }
6459 
6460 void MainWindow::showPostProcessingDialog()
6461 {
6462  if(_postProcessingDialog->exec() != QDialog::Accepted)
6463  {
6464  return;
6465  }
6466 
6467  postProcessing(
6468  _postProcessingDialog->isRefineNeighborLinks(),
6469  _postProcessingDialog->isRefineLoopClosureLinks(),
6470  _postProcessingDialog->isDetectMoreLoopClosures(),
6471  _postProcessingDialog->clusterRadius(),
6472  _postProcessingDialog->clusterAngle(),
6473  _postProcessingDialog->iterations(),
6474  _postProcessingDialog->interSession(),
6475  _postProcessingDialog->intraSession(),
6476  _postProcessingDialog->isSBA(),
6477  _postProcessingDialog->sbaIterations(),
6478  _postProcessingDialog->sbaVariance(),
6479  _postProcessingDialog->sbaType(),
6480  _postProcessingDialog->sbaRematchFeatures());
6481 }
6482 
6483 void MainWindow::postProcessing(
6484  bool refineNeighborLinks,
6485  bool refineLoopClosureLinks,
6486  bool detectMoreLoopClosures,
6487  double clusterRadius,
6488  double clusterAngle,
6489  int iterations,
6490  bool interSession,
6491  bool intraSession,
6492  bool sba,
6493  int sbaIterations,
6494  double sbaVariance,
6495  Optimizer::Type sbaType,
6496  double sbaRematchFeatures,
6497  bool abortIfDataMissing)
6498 {
6499  if(_cachedSignatures.size() == 0)
6500  {
6501  QMessageBox::warning(this,
6502  tr("Post-processing..."),
6503  tr("Signatures must be cached in the GUI for post-processing. "
6504  "Check the option in Preferences->General Settings (GUI), then "
6505  "refresh the cache."));
6506  return;
6507  }
6508 
6509  if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
6510  {
6511  UWARN("No post-processing selection...");
6512  return;
6513  }
6514 
6515  if(_currentPosesMap.lower_bound(1) == _currentPosesMap.end())
6516  {
6517  UWARN("No nodes to process...");
6518  return;
6519  }
6520 
6521  // First, verify that we have all data required in the GUI
6522  std::map<int, Transform> odomPoses;
6523  bool allDataAvailable = true;
6524  for(std::map<int, Transform>::iterator iter = _currentPosesMap.lower_bound(1);
6525  iter!=_currentPosesMap.end() && allDataAvailable;
6526  ++iter)
6527  {
6528  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
6529  if(jter == _cachedSignatures.end())
6530  {
6531  UWARN("Node %d missing.", iter->first);
6532  allDataAvailable = false;
6533  }
6534  else if(!jter.value().getPose().isNull())
6535  {
6536  odomPoses.insert(std::make_pair(iter->first, jter.value().getPose()));
6537  }
6538  }
6539 
6540  if(!allDataAvailable)
6541  {
6542  QString msg = tr("Some data missing in the cache to respect the constraints chosen. "
6543  "Try \"Edit->Download all clouds\" to update the cache and try again.");
6544  UWARN(msg.toStdString().c_str());
6545  if(abortIfDataMissing)
6546  {
6547  QMessageBox::warning(this, tr("Not all data available in the GUI..."), msg);
6548  return;
6549  }
6550  }
6551 
6552  _progressDialog->resetProgress();
6553  _progressDialog->clear();
6554  _progressDialog->show();
6555  _progressDialog->appendText("Post-processing beginning!");
6556  _progressDialog->setCancelButtonVisible(true);
6557  _progressCanceled = false;
6558 
6559  int totalSteps = 0;
6560  if(refineNeighborLinks)
6561  {
6562  totalSteps+=(int)_currentPosesMap.size();
6563  }
6564  if(refineLoopClosureLinks)
6565  {
6566  totalSteps+=(int)_currentLinksMap.size() - (int)_currentPosesMap.size();
6567  }
6568  if(sba)
6569  {
6570  totalSteps+=1;
6571  }
6572  _progressDialog->setMaximumSteps(totalSteps);
6573  _progressDialog->show();
6574 
6575  ParametersMap parameters = _preferencesDialog->getAllParameters();
6576  Optimizer * optimizer = Optimizer::create(parameters);
6577  bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
6578  float optimizeMaxError = Parameters::defaultRGBDOptimizeMaxError();
6579  int optimizeIterations = Parameters::defaultOptimizerIterations();
6580  bool reextractFeatures = Parameters::defaultRGBDLoopClosureReextractFeatures();
6581  Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
6582  Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), optimizeMaxError);
6583  Parameters::parse(parameters, Parameters::kOptimizerIterations(), optimizeIterations);
6584  Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), reextractFeatures);
6585 
6586  bool warn = false;
6587  int loopClosuresAdded = 0;
6588  std::multimap<int, int> checkedLoopClosures;
6589  if(detectMoreLoopClosures)
6590  {
6591  UDEBUG("");
6592 
6593  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
6594  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
6595  std::vector<double> odomMaxInf;
6596  if(loopCovLimited)
6597  {
6598  odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
6599  }
6600 
6601  UASSERT(iterations>0);
6602  for(int n=0; n<iterations && !_progressCanceled; ++n)
6603  {
6604  _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
6605  .arg(n+1).arg(iterations).arg(clusterRadius).arg(clusterAngle));
6606 
6607  std::multimap<int, int> clusters = graph::radiusPosesClustering(
6608  std::map<int, Transform>(_currentPosesMap.upper_bound(0), _currentPosesMap.end()),
6609  clusterRadius,
6610  clusterAngle*CV_PI/180.0);
6611 
6612  _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+(int)clusters.size());
6613  _progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
6614  QApplication::processEvents();
6615 
6616  int i=0;
6617  std::set<int> addedLinks;
6618  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)
6619  {
6620  int from = iter->first;
6621  int to = iter->second;
6622  if(iter->first < iter->second)
6623  {
6624  from = iter->second;
6625  to = iter->first;
6626  }
6627 
6628  int mapIdFrom = uValue(_currentMapIds, from, 0);
6629  int mapIdTo = uValue(_currentMapIds, to, 0);
6630 
6631  if((interSession && mapIdFrom != mapIdTo) ||
6632  (intraSession && mapIdFrom == mapIdTo))
6633  {
6634  bool alreadyChecked = false;
6635  for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
6636  !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
6637  ++jter)
6638  {
6639  if(to == jter->second)
6640  {
6641  alreadyChecked = true;
6642  }
6643  }
6644 
6645  if(!alreadyChecked)
6646  {
6647  // only add new links and one per cluster per iteration
6648  if(addedLinks.find(from) == addedLinks.end() &&
6649  addedLinks.find(to) == addedLinks.end() &&
6650  rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
6651  {
6652  // Reverify if in the bounds with the current optimized graph
6653  Transform delta = _currentPosesMap.at(from).inverse() * _currentPosesMap.at(to);
6654  if(delta.getNorm() < clusterRadius)
6655  {
6656  checkedLoopClosures.insert(std::make_pair(from, to));
6657 
6658  if(!_cachedSignatures.contains(from))
6659  {
6660  UERROR("Didn't find signature %d", from);
6661  }
6662  else if(!_cachedSignatures.contains(to))
6663  {
6664  UERROR("Didn't find signature %d", to);
6665  }
6666  else
6667  {
6668  Signature signatureFrom = _cachedSignatures[from];
6669  Signature signatureTo = _cachedSignatures[to];
6670 
6671  if(signatureFrom.getWeight() >= 0 &&
6672  signatureTo.getWeight() >= 0) // ignore intermediate nodes
6673  {
6674  Transform transform;
6675  RegistrationInfo info;
6676  if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
6677  parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
6678  {
6679  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
6680  }
6681  Registration * registration = Registration::create(parameters);
6682 
6683  if(reextractFeatures)
6684  {
6685  signatureFrom.sensorData().uncompressData();
6686  signatureTo.sensorData().uncompressData();
6687 
6688  if(signatureFrom.sensorData().imageRaw().empty() &&
6689  signatureTo.sensorData().imageRaw().empty())
6690  {
6691  UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
6692  "images. Update the cache.",
6693  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
6694  }
6695  else
6696  {
6697  signatureFrom.removeAllWords();
6698  signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
6699  signatureTo.removeAllWords();
6700  signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
6701  }
6702  }
6703  else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
6704  {
6705  UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
6706  "registration will not be possible. Set \"%s\" to true.",
6707  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
6708  signatureFrom.id(),
6709  signatureTo.id(),
6710  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
6711  }
6712  transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
6713  delete registration;
6714  if(!transform.isNull())
6715  {
6716  //optimize the graph to see if the new constraint is globally valid
6717  bool updateConstraint = true;
6718  cv::Mat information = info.covariance.inv();
6719  if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
6720  {
6721  for(int i=0; i<6; ++i)
6722  {
6723  if(information.at<double>(i,i) > odomMaxInf[i])
6724  {
6725  information.at<double>(i,i) = odomMaxInf[i];
6726  }
6727  }
6728  }
6729  if(optimizeMaxError > 0.0f && optimizeIterations > 0)
6730  {
6731  int fromId = from;
6732  int mapId = _currentMapIds.at(from);
6733  // use first node of the map containing from
6734  for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
6735  {
6736  if(iter->second == mapId && _currentPosesMap.find(iter->first)!=_currentPosesMap.end())
6737  {
6738  fromId = iter->first;
6739  break;
6740  }
6741  }
6742  std::multimap<int, Link> linksIn = _currentLinksMap;
6743  linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
6744  const Link * maxLinearLink = 0;
6745  const Link * maxAngularLink = 0;
6746  float maxLinearError = 0.0f;
6747  float maxAngularError = 0.0f;
6748  std::map<int, Transform> poses;
6749  std::multimap<int, Link> links;
6750  UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
6751  UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
6752  UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
6753  optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
6754  UASSERT(poses.find(fromId) != poses.end());
6755  UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
6756  UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
6757  UASSERT(graph::findLink(links, from, to) != links.end());
6758  poses = optimizer->optimize(fromId, poses, links);
6759  std::string msg;
6760  if(poses.size())
6761  {
6762  float maxLinearErrorRatio = 0.0f;
6763  float maxAngularErrorRatio = 0.0f;
6764  graph::computeMaxGraphErrors(
6765  poses,
6766  links,
6767  maxLinearErrorRatio,
6768  maxAngularErrorRatio,
6769  maxLinearError,
6770  maxAngularError,
6771  &maxLinearLink,
6772  &maxAngularLink);
6773  if(maxLinearLink)
6774  {
6775  UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
6776  if(maxLinearErrorRatio > optimizeMaxError)
6777  {
6778  msg = uFormat("Rejecting edge %d->%d because "
6779  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
6780  "\"%s\" is %f.",
6781  from,
6782  to,
6783  maxLinearError,
6784  maxLinearLink->from(),
6785  maxLinearLink->to(),
6786  maxLinearErrorRatio,
6787  sqrt(maxLinearLink->transVariance()),
6788  Parameters::kRGBDOptimizeMaxError().c_str(),
6789  optimizeMaxError);
6790  }
6791  }
6792  else if(maxAngularLink)
6793  {
6794  UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
6795  if(maxAngularErrorRatio > optimizeMaxError)
6796  {
6797  msg = uFormat("Rejecting edge %d->%d because "
6798  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
6799  "\"%s\" is %f m.",
6800  from,
6801  to,
6802  maxAngularError*180.0f/M_PI,
6803  maxAngularLink->from(),
6804  maxAngularLink->to(),
6805  maxAngularErrorRatio,
6806  sqrt(maxAngularLink->rotVariance()),
6807  Parameters::kRGBDOptimizeMaxError().c_str(),
6808  optimizeMaxError);
6809  }
6810  }
6811  }
6812  else
6813  {
6814  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
6815  from,
6816  to);
6817  }
6818  if(!msg.empty())
6819  {
6820  UWARN("%s", msg.c_str());
6821  _progressDialog->appendText(tr("%1").arg(msg.c_str()));
6822  QApplication::processEvents();
6823  updateConstraint = false;
6824  }
6825  else
6826  {
6827  _currentPosesMap = poses;
6828  }
6829  }
6830 
6831  if(updateConstraint)
6832  {
6833  UINFO("Added new loop closure between %d and %d.", from, to);
6834  addedLinks.insert(from);
6835  addedLinks.insert(to);
6836 
6837  _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
6838  ++loopClosuresAdded;
6839  _progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
6840  }
6841  }
6842  }
6843  }
6844  }
6845  }
6846  }
6847  }
6848  QApplication::processEvents();
6849  _progressDialog->incrementStep();
6850  }
6851  _progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
6852  if(addedLinks.size() == 0)
6853  {
6854  break;
6855  }
6856 
6857  if(n+1 < iterations)
6858  {
6859  _progressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
6860  .arg(_currentPosesMap.size()).arg(_currentLinksMap.size()));
6861  QApplication::processEvents();
6862 
6863  UASSERT(_currentPosesMap.lower_bound(1) != _currentPosesMap.end());
6864  int fromId = optimizeFromGraphEnd?_currentPosesMap.rbegin()->first:_currentPosesMap.lower_bound(1)->first;
6865  std::map<int, rtabmap::Transform> posesOut;
6866  std::multimap<int, rtabmap::Link> linksOut;
6867  std::map<int, rtabmap::Transform> optimizedPoses;
6868  std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
6869  optimizer->getConnectedGraph(
6870  fromId,
6871  _currentPosesMap,
6872  linksIn,
6873  posesOut,
6874  linksOut);
6875  optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
6876  _currentPosesMap = optimizedPoses;
6877  _progressDialog->appendText(tr("Optimizing graph with new links... done!"));
6878  }
6879  }
6880  UINFO("Added %d loop closures.", loopClosuresAdded);
6881  _progressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
6882  }
6883 
6884  if(!_progressCanceled && (refineNeighborLinks || refineLoopClosureLinks))
6885  {
6886  UDEBUG("");
6887  if(refineLoopClosureLinks)
6888  {
6889  _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+loopClosuresAdded);
6890  }
6891  // TODO: support ICP from laser scans?
6892  _progressDialog->appendText(tr("Refining links..."));
6893  QApplication::processEvents();
6894 
6895  RegistrationIcp regIcp(parameters);
6896 
6897  int i=0;
6898  for(std::multimap<int, Link>::iterator iter = _currentLinksMap.lower_bound(1); iter!=_currentLinksMap.end() && !_progressCanceled; ++iter, ++i)
6899  {
6900  int type = iter->second.type();
6901  int from = iter->second.from();
6902  int to = iter->second.to();
6903 
6904  if((refineNeighborLinks && type==Link::kNeighbor) ||
6905  (refineLoopClosureLinks && type!=Link::kNeighbor && type!=Link::kLandmark && from!=to))
6906  {
6907  _progressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
6908  _progressDialog->incrementStep();
6909  QApplication::processEvents();
6910 
6911  if(!_cachedSignatures.contains(from))
6912  {
6913  UERROR("Didn't find signature %d",from);
6914  }
6915  else if(!_cachedSignatures.contains(to))
6916  {
6917  UERROR("Didn't find signature %d", to);
6918  }
6919  else
6920  {
6921  Signature & signatureFrom = _cachedSignatures[from];
6922  Signature & signatureTo = _cachedSignatures[to];
6923 
6924  LaserScan tmp;
6925  signatureFrom.sensorData().uncompressData(0,0,&tmp);
6926  signatureTo.sensorData().uncompressData(0,0,&tmp);
6927 
6928  if(!signatureFrom.sensorData().laserScanRaw().isEmpty() &&
6929  !signatureTo.sensorData().laserScanRaw().isEmpty())
6930  {
6931  RegistrationInfo info;
6932  Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
6933 
6934  if(!transform.isNull())
6935  {
6936  Link newLink(from, to, iter->second.type(), transform, info.covariance.inv());
6937  iter->second = newLink;
6938  }
6939  else
6940  {
6941  QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg.c_str());
6942  _progressDialog->appendText(str, Qt::darkYellow);
6943  UWARN("%s", str.toStdString().c_str());
6944  warn = true;
6945  }
6946  }
6947  else
6948  {
6949  QString str;
6950  if(signatureFrom.getWeight() < 0 || signatureTo.getWeight() < 0)
6951  {
6952  str = tr("Cannot refine link %1->%2 (Intermediate node detected!)").arg(from).arg(to);
6953  }
6954  else
6955  {
6956  str = tr("Cannot refine link %1->%2 (scans empty!)").arg(from).arg(to);
6957  }
6958 
6959  _progressDialog->appendText(str, Qt::darkYellow);
6960  UWARN("%s", str.toStdString().c_str());
6961  warn = true;
6962  }
6963  }
6964  }
6965  }
6966  _progressDialog->appendText(tr("Refining links...done!"));
6967  }
6968 
6969  _progressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
6970  .arg(_currentPosesMap.size()).arg(_currentLinksMap.size()));
6971 
6972  UASSERT(_currentPosesMap.lower_bound(1) != _currentPosesMap.end());
6973  int fromId = optimizeFromGraphEnd?_currentPosesMap.rbegin()->first:_currentPosesMap.lower_bound(1)->first;
6974  std::map<int, rtabmap::Transform> posesOut;
6975  std::multimap<int, rtabmap::Link> linksOut;
6976  std::map<int, rtabmap::Transform> optimizedPoses;
6977  std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
6978  optimizer->getConnectedGraph(
6979  fromId,
6980  _currentPosesMap,
6981  linksIn,
6982  posesOut,
6983  linksOut);
6984  optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
6985  _progressDialog->appendText(tr("Optimizing graph with updated links... done!"));
6986  _progressDialog->incrementStep();
6987 
6988  if(!_progressCanceled && sba)
6989  {
6990  UASSERT(Optimizer::isAvailable(sbaType));
6991  _progressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
6992  .arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
6993  QApplication::processEvents();
6994  uSleep(100);
6995  QApplication::processEvents();
6996 
6997  ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
6998  uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
6999  uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
7000  Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
7001  std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
7002  delete sbaOptimizer;
7003  if(newPoses.size())
7004  {
7005  optimizedPoses = newPoses;
7006  _progressDialog->appendText(tr("SBA... done!"));
7007  }
7008  else
7009  {
7010  _progressDialog->appendText(tr("SBA... failed!"));
7011  _progressDialog->setAutoClose(false);
7012  }
7013  _progressDialog->incrementStep();
7014  }
7015 
7016  _progressDialog->appendText(tr("Updating map..."));
7017  this->updateMapCloud(
7018  optimizedPoses,
7019  std::multimap<int, Link>(_currentLinksMap),
7020  std::map<int, int>(_currentMapIds),
7021  std::map<int, std::string>(_currentLabels),
7022  std::map<int, Transform>(_currentGTPosesMap),
7023  std::map<int, Transform>(),
7024  std::multimap<int, Link>(),
7025  false);
7026  _progressDialog->appendText(tr("Updating map... done!"));
7027 
7028  if(warn)
7029  {
7030  _progressDialog->setAutoClose(false);
7031  }
7032 
7033  _progressDialog->setValue(_progressDialog->maximumSteps());
7034  _progressDialog->appendText("Post-processing finished!");
7035  _progressDialog->setCancelButtonVisible(false);
7036  _progressCanceled = false;
7037 
7038  delete optimizer;
7039 }
7040 
7041 void MainWindow::depthCalibration()
7042 {
7043  if(_currentPosesMap.size() && _cachedSignatures.size())
7044  {
7045  _depthCalibrationDialog->calibrate(
7046  _currentPosesMap,
7047  _cachedSignatures,
7048  _preferencesDialog->getWorkingDirectory(),
7049  _preferencesDialog->getAllParameters());
7050  }
7051  else
7052  {
7053  QMessageBox::warning(this, tr("Depth Calibration"), tr("No data in cache. Try to refresh the cache."));
7054  }
7055 }
7056 
7057 void MainWindow::deleteMemory()
7058 {
7059  QMessageBox::StandardButton button;
7060  if(_state == kMonitoring || _state == kMonitoringPaused)
7061  {
7062  button = QMessageBox::question(this,
7063  tr("Deleting memory..."),
7064  tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
7065  QMessageBox::Yes|QMessageBox::No,
7066  QMessageBox::No);
7067  }
7068  else
7069  {
7070  button = QMessageBox::question(this,
7071  tr("Deleting memory..."),
7072  tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
7073  QMessageBox::Yes|QMessageBox::No,
7074  QMessageBox::No);
7075  }
7076 
7077  if(button != QMessageBox::Yes)
7078  {
7079  return;
7080  }
7081 
7082  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
7083  if(_state!=kDetecting)
7084  {
7085  _databaseUpdated = false;
7086  }
7087  this->clearTheCache();
7088 }
7089 
7090 QString MainWindow::getWorkingDirectory() const
7091 {
7092  return _preferencesDialog->getWorkingDirectory();
7093 }
7094 
7095 void MainWindow::openWorkingDirectory()
7096 {
7097  QString filePath = _preferencesDialog->getWorkingDirectory();
7098 #if defined(Q_WS_MAC)
7099  QStringList args;
7100  args << "-e";
7101  args << "tell application \"Finder\"";
7102  args << "-e";
7103  args << "activate";
7104  args << "-e";
7105  args << "select POSIX file \""+filePath+"\"";
7106  args << "-e";
7107  args << "end tell";
7108  QProcess::startDetached("osascript", args);
7109 #elif defined(Q_WS_WIN)
7110  QStringList args;
7111  args << "/select," << QDir::toNativeSeparators(filePath);
7112  QProcess::startDetached("explorer", args);
7113 #else
7114  UERROR("Only works on Mac and Windows");
7115 #endif
7116 }
7117 
7118 void MainWindow::updateEditMenu()
7119 {
7120  // Update Memory delete database size
7121  if(_state != kMonitoring && _state != kMonitoringPaused && (!_openedDatabasePath.isEmpty() || !_newDatabasePath.isEmpty()))
7122  {
7123  if(!_openedDatabasePath.isEmpty())
7124  {
7125  _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_openedDatabasePath.toStdString())/1000000));
7126  }
7127  else
7128  {
7129  _ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(_newDatabasePath.toStdString())/1000000));
7130  }
7131  }
7132 }
7133 
7134 void MainWindow::selectOpenni()
7135 {
7136  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
7137 }
7138 
7139 void MainWindow::selectFreenect()
7140 {
7141  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
7142 }
7143 
7144 void MainWindow::selectOpenniCv()
7145 {
7146  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
7147 }
7148 
7149 void MainWindow::selectOpenniCvAsus()
7150 {
7151  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
7152 }
7153 
7154 void MainWindow::selectOpenni2()
7155 {
7156  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
7157 }
7158 
7159 void MainWindow::selectFreenect2()
7160 {
7161  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
7162 }
7163 
7164 void MainWindow::selectK4W2()
7165 {
7166  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
7167 }
7168 
7169 void MainWindow::selectK4A()
7170 {
7171  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4A);
7172 }
7173 
7174 void MainWindow::selectRealSense()
7175 {
7176  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);
7177 }
7178 
7179 void MainWindow::selectRealSense2()
7180 {
7181  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2);
7182 }
7183 void MainWindow::selectRealSense2L515()
7184 {
7185  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2, 1);
7186 }
7187 
7188 void MainWindow::selectRealSense2Stereo()
7189 {
7190  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoRealSense2);
7191 }
7192 
7193 void MainWindow::selectStereoDC1394()
7194 {
7195  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
7196 }
7197 
7198 void MainWindow::selectStereoFlyCapture2()
7199 {
7200  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
7201 }
7202 void MainWindow::selectStereoZed()
7203 {
7204  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed);
7205 }
7206 void MainWindow::selectStereoZedOC()
7207 {
7208  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZedOC);
7209 }
7210 
7211 void MainWindow::selectStereoTara()
7212 {
7213  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoTara);
7214 }
7215 
7216 void MainWindow::selectStereoUsb()
7217 {
7218  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
7219 }
7220 
7221 void MainWindow::selectMyntEyeS()
7222 {
7223  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoMyntEye);
7224 }
7225 
7226 void MainWindow::selectDepthAIOAKD()
7227 {
7228  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 1); // variant 1=IMU
7229 }
7230 
7231 void MainWindow::selectDepthAIOAKDLite()
7232 {
7233  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 0); // variant 0=no IMU
7234 }
7235 
7236 void MainWindow::selectDepthAIOAKDPro()
7237 {
7238  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 2); // variant 2=IMU+color
7239 }
7240 
7241 void MainWindow::selectXvisioSeerSense()
7242 {
7243  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcSeerSense);
7244 }
7245 
7246 void MainWindow::selectVLP16()
7247 {
7248  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16);
7249 }
7250 
7251 void MainWindow::dumpTheMemory()
7252 {
7253  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
7254 }
7255 
7256 void MainWindow::dumpThePrediction()
7257 {
7258  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
7259 }
7260 
7261 void MainWindow::sendGoal()
7262 {
7263  UINFO("Sending a goal...");
7264  bool ok = false;
7265  QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
7266  if(ok && !text.isEmpty())
7267  {
7268  _waypoints.clear();
7269  _waypointsIndex = 0;
7270 
7271  this->postGoal(text);
7272  }
7273 }
7274 
7275 void MainWindow::sendWaypoints()
7276 {
7277  UINFO("Sending waypoints...");
7278  bool ok = false;
7279  QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
7280  if(ok && !text.isEmpty())
7281  {
7282  QStringList wp = text.split(' ');
7283  if(wp.size() < 2)
7284  {
7285  QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
7286  }
7287  else
7288  {
7289  _waypoints = wp;
7290  _waypointsIndex = 0;
7291  this->postGoal(_waypoints.at(_waypointsIndex));
7292  }
7293  }
7294 }
7295 
7296 void MainWindow::postGoal()
7297 {
7298  if(!_waypoints.isEmpty())
7299  {
7300  postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
7301  }
7302 }
7303 
7304 void MainWindow::postGoal(const QString & goal)
7305 {
7306  if(!goal.isEmpty())
7307  {
7308  bool ok = false;
7309  int id = goal.toInt(&ok);
7310  _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
7311  UINFO("Posting event with goal %s", goal.toStdString().c_str());
7312  if(ok)
7313  {
7314  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
7315  }
7316  else
7317  {
7318  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
7319  }
7320  }
7321 }
7322 
7323 void MainWindow::cancelGoal()
7324 {
7325  UINFO("Cancelling goal...");
7326  _waypoints.clear();
7327  _waypointsIndex = 0;
7328  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
7329 }
7330 
7331 void MainWindow::label()
7332 {
7333  UINFO("Labelling current location...");
7334  bool ok = false;
7335  QString label = QInputDialog::getText(this, tr("Label current location"), tr("Label: "), QLineEdit::Normal, "", &ok);
7336  if(ok && !label.isEmpty())
7337  {
7338  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdLabel, label.toStdString(), 0));
7339  }
7340 }
7341 
7342 void MainWindow::removeLabel()
7343 {
7344  UINFO("Removing label...");
7345  bool ok = false;
7346  QString label = QInputDialog::getText(this, tr("Remove label"), tr("Label: "), QLineEdit::Normal, "", &ok);
7347  if(ok && !label.isEmpty())
7348  {
7349  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRemoveLabel, label.toStdString(), 0));
7350  }
7351 }
7352 
7353 void MainWindow::updateCacheFromDatabase()
7354 {
7355  QString dir = getWorkingDirectory();
7356  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
7357  if(!path.isEmpty())
7358  {
7359  updateCacheFromDatabase(path);
7360  }
7361 }
7362 
7363 void MainWindow::updateCacheFromDatabase(const QString & path)
7364 {
7365  if(!path.isEmpty())
7366  {
7367  DBDriver * driver = DBDriver::create();
7368  if(driver->openConnection(path.toStdString()))
7369  {
7370  UINFO("Update cache...");
7371  _progressDialog->resetProgress();
7372  _progressDialog->show();
7373  _progressDialog->appendText(tr("Downloading the map from \"%1\" (without poses and links)...")
7374  .arg(path));
7375 
7376  std::set<int> ids;
7377  driver->getAllNodeIds(ids, true);
7378  std::list<Signature*> signaturesList;
7379  driver->loadSignatures(std::list<int>(ids.begin(), ids.end()), signaturesList);
7380  std::map<int, Signature> signatures;
7381  driver->loadNodeData(signaturesList);
7382  for(std::list<Signature *>::iterator iter=signaturesList.begin(); iter!=signaturesList.end(); ++iter)
7383  {
7384  signatures.insert(std::make_pair((*iter)->id(), *(*iter)));
7385  delete *iter;
7386  }
7387  if(_currentPosesMap.empty() && _currentLinksMap.empty())
7388  {
7389  _currentPosesMap = driver->loadOptimizedPoses();
7390  driver->getAllLinks(_currentLinksMap, true, true);
7391  }
7392  RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap);
7393  processRtabmapEvent3DMap(event);
7394  }
7395  else
7396  {
7397  QMessageBox::warning(this, tr("Update cache"), tr("Failed to open database \"%1\"").arg(path));
7398  }
7399  delete driver;
7400  }
7401 }
7402 
7403 void MainWindow::downloadAllClouds()
7404 {
7405  QStringList items;
7406  items.append("Local map optimized");
7407  items.append("Local map not optimized");
7408  items.append("Global map optimized");
7409  items.append("Global map not optimized");
7410 
7411  bool ok;
7412  QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 0, false, &ok);
7413  if(ok)
7414  {
7415  bool optimized=false, global=false;
7416  if(item.compare("Local map optimized") == 0)
7417  {
7418  optimized = true;
7419  }
7420  else if(item.compare("Local map not optimized") == 0)
7421  {
7422 
7423  }
7424  else if(item.compare("Global map optimized") == 0)
7425  {
7426  global=true;
7427  optimized=true;
7428  }
7429  else if(item.compare("Global map not optimized") == 0)
7430  {
7431  global=true;
7432  }
7433  else
7434  {
7435  UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
7436  }
7437 
7438  UINFO("Download clouds...");
7439  _progressDialog->resetProgress();
7440  _progressDialog->show();
7441  _progressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
7442  .arg(global?"true":"false").arg(optimized?"true":"false"));
7443  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
7444  }
7445 }
7446 
7447 void MainWindow::downloadPoseGraph()
7448 {
7449  QStringList items;
7450  items.append("Local map optimized");
7451  items.append("Local map not optimized");
7452  items.append("Global map optimized");
7453  items.append("Global map not optimized");
7454 
7455  bool ok;
7456  QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 0, false, &ok);
7457  if(ok)
7458  {
7459  bool optimized=false, global=false;
7460  if(item.compare("Local map optimized") == 0)
7461  {
7462  optimized = true;
7463  }
7464  else if(item.compare("Local map not optimized") == 0)
7465  {
7466 
7467  }
7468  else if(item.compare("Global map optimized") == 0)
7469  {
7470  global=true;
7471  optimized=true;
7472  }
7473  else if(item.compare("Global map not optimized") == 0)
7474  {
7475  global=true;
7476  }
7477  else
7478  {
7479  UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
7480  }
7481 
7482  UINFO("Download the graph...");
7483  _progressDialog->resetProgress();
7484  _progressDialog->show();
7485  _progressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
7486  .arg(global?"true":"false").arg(optimized?"true":"false"));
7487 
7488  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
7489  }
7490 }
7491 
7492 void MainWindow::anchorCloudsToGroundTruth()
7493 {
7494  this->updateMapCloud(
7495  std::map<int, Transform>(_currentPosesMap),
7496  std::multimap<int, Link>(_currentLinksMap),
7497  std::map<int, int>(_currentMapIds),
7498  std::map<int, std::string>(_currentLabels),
7499  std::map<int, Transform>(_currentGTPosesMap));
7500 }
7501 
7502 void MainWindow::clearTheCache()
7503 {
7504  _cachedSignatures.clear();
7505  _cachedMemoryUsage = 0;
7506  _cachedWordsCount.clear();
7507  _cachedClouds.clear();
7508  _createdCloudsMemoryUsage = 0;
7509  _cachedEmptyClouds.clear();
7510  _previousCloud.first = 0;
7511  _previousCloud.second.first.first.reset();
7512  _previousCloud.second.first.second.reset();
7513  _previousCloud.second.second.reset();
7514  _createdScans.clear();
7515  _createdFeatures.clear();
7516  _cloudViewer->clear();
7517  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
7518  _cloudViewer->clearTrajectory();
7519  _ui->widget_mapVisibility->clear();
7520  _currentPosesMap.clear();
7521  _currentGTPosesMap.clear();
7522  _currentLinksMap.clear();
7523  _currentMapIds.clear();
7524  _currentLabels.clear();
7525  _odometryCorrection = Transform::getIdentity();
7526  _lastOdomPose.setNull();
7527  _ui->statsToolBox->clear();
7528  //disable save cloud action
7529  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
7530  _ui->actionPost_processing->setEnabled(false);
7531  _ui->actionSave_point_cloud->setEnabled(false);
7532  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
7533  _ui->actionDepth_Calibration->setEnabled(false);
7534  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
7535  _ui->actionExport_octomap->setEnabled(false);
7536  _ui->actionView_high_res_point_cloud->setEnabled(false);
7537  _likelihoodCurve->clear();
7538  _rawLikelihoodCurve->clear();
7539  _posteriorCurve->clear();
7540  _lastId = 0;
7541  _lastIds.clear();
7542  _firstStamp = 0.0f;
7543  _ui->label_stats_loopClosuresDetected->setText("0");
7544  _ui->label_stats_loopClosuresReactivatedDetected->setText("0");
7545  _ui->label_stats_loopClosuresRejected->setText("0");
7546  _refIds.clear();
7547  _loopClosureIds.clear();
7548  _cachedLocalizationsCount.clear();
7549  _ui->label_refId->clear();
7550  _ui->label_matchId->clear();
7551  _ui->graphicsView_graphView->clearAll();
7552  _ui->imageView_source->clear();
7553  _ui->imageView_loopClosure->clear();
7554  _ui->imageView_odometry->clear();
7555  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
7556  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
7557  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
7558  _multiSessionLocWidget->clear();
7559  _cachedLocalMaps.clear();
7560 #ifdef RTABMAP_OCTOMAP
7561  // re-create one if the resolution has changed
7562  delete _octomap;
7563  _octomap = new OctoMap(&_cachedLocalMaps, _preferencesDialog->getAllParameters());
7564 #endif
7565  _occupancyGrid->clear();
7566  _rectCameraModels.clear();
7567  _rectCameraModelsOdom.clear();
7568 }
7569 
7570 void MainWindow::openHelp()
7571 {
7572  if(_state == kMonitoringPaused || _state == kMonitoring)
7573  {
7574  QDesktopServices::openUrl(QUrl("http://wiki.ros.org/rtabmap_ros"));
7575  }
7576  else
7577  {
7578  QDesktopServices::openUrl(QUrl("https://github.com/introlab/rtabmap/wiki"));
7579  }
7580 }
7581 
7582 void MainWindow::updateElapsedTime()
7583 {
7584  if(_state == kDetecting || _state == kMonitoring)
7585  {
7586  QString format = "hh:mm:ss";
7587  _ui->label_elapsedTime->setText((QTime().fromString(_ui->label_elapsedTime->text(), format).addMSecs(_elapsedTime->restart())).toString(format));
7588  }
7589 }
7590 
7591 void MainWindow::saveFigures()
7592 {
7593  QList<int> curvesPerFigure;
7594  QStringList curveNames;
7595  QStringList curveThresholds;
7596  _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames, curveThresholds);
7597 
7598  QStringList curvesPerFigureStr;
7599  for(int i=0; i<curvesPerFigure.size(); ++i)
7600  {
7601  curvesPerFigureStr.append(QString::number(curvesPerFigure[i]));
7602  }
7603  for(int i=0; i<curveNames.size(); ++i)
7604  {
7605  curveNames[i].replace(' ', '_');
7606  }
7607  _preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
7608  _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
7609  _preferencesDialog->saveCustomConfig("Figures", "thresholds", curveThresholds.join(" "));
7610 }
7611 
7612 void MainWindow::loadFigures()
7613 {
7614  QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
7615  QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
7616  QString curveThresholds = _preferencesDialog->loadCustomConfig("Figures", "thresholds");
7617 
7618  if(!curvesPerFigure.isEmpty())
7619  {
7620  QStringList curvesPerFigureList = curvesPerFigure.split(" ");
7621  QStringList curvesNamesList = curveNames.split(" ");
7622  QStringList curveThresholdsList = curveThresholds.split(" ");
7623  if(curveThresholdsList[0].isEmpty()) {
7624  curveThresholdsList.clear();
7625  }
7626  UASSERT(curveThresholdsList.isEmpty() || curveThresholdsList.size() == curvesNamesList.size());
7627 
7628  int j=0;
7629  for(int i=0; i<curvesPerFigureList.size(); ++i)
7630  {
7631  bool ok = false;
7632  int count = curvesPerFigureList[i].toInt(&ok);
7633  if(!ok)
7634  {
7635  QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
7636  break;
7637  }
7638  else
7639  {
7640  _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
7641  for(int k=1; k<count && j<curveNames.size(); ++k)
7642  {
7643  if(curveThresholdsList.size())
7644  {
7645  bool ok = false;
7646  double thresholdValue = curveThresholdsList[j].toDouble(&ok);
7647  if(ok)
7648  {
7649  _ui->statsToolBox->addThreshold(curvesNamesList[j++].replace('_', ' '), thresholdValue);
7650  }
7651  else
7652  {
7653  _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
7654  }
7655  }
7656  else
7657  {
7658  _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
7659  }
7660  }
7661  }
7662  }
7663  }
7664 
7665 }
7666 
7667 void MainWindow::openPreferences()
7668 {
7669  _preferencesDialog->setMonitoringState(_state == kMonitoring || _state == kMonitoringPaused);
7670  _preferencesDialog->exec();
7671 }
7672 
7673 void MainWindow::openPreferencesSource()
7674 {
7675  _preferencesDialog->setCurrentPanelToSource();
7676  openPreferences();
7677  this->updateSelectSourceMenu();
7678 }
7679 
7680 void MainWindow::setDefaultViews()
7681 {
7682  _ui->dockWidget_posterior->setVisible(false);
7683  _ui->dockWidget_likelihood->setVisible(false);
7684  _ui->dockWidget_rawlikelihood->setVisible(false);
7685  _ui->dockWidget_statsV2->setVisible(false);
7686  _ui->dockWidget_console->setVisible(false);
7687  _ui->dockWidget_loopClosureViewer->setVisible(false);
7688  _ui->dockWidget_mapVisibility->setVisible(false);
7689  _ui->dockWidget_graphViewer->setVisible(false);
7690  _ui->dockWidget_odometry->setVisible(true);
7691  _ui->dockWidget_cloudViewer->setVisible(true);
7692  _ui->dockWidget_imageView->setVisible(true);
7693  _ui->dockWidget_multiSessionLoc->setVisible(false);
7694  _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused);
7695  _ui->toolBar_2->setVisible(true);
7696  _ui->statusbar->setVisible(false);
7697  this->setAspectRatio720p();
7698  _cloudViewer->resetCamera();
7699  _cloudViewer->setCameraLockZ(true);
7700  _cloudViewer->setCameraTargetFollow(true);
7701 }
7702 
7703 void MainWindow::selectScreenCaptureFormat(bool checked)
7704 {
7705  if(checked)
7706  {
7707  QStringList items;
7708  items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
7709  bool ok;
7710  QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
7711  if(ok && !item.isEmpty())
7712  {
7713  if(item.compare("Synchronize with map update") == 0)
7714  {
7715  _autoScreenCaptureOdomSync = false;
7716  }
7717  else
7718  {
7719  _autoScreenCaptureOdomSync = true;
7720  }
7721 
7722  if(_state != kMonitoring && _state != kMonitoringPaused)
7723  {
7724  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);
7725  if(r == QMessageBox::No || r == QMessageBox::Yes)
7726  {
7727  _autoScreenCaptureRAM = r == QMessageBox::Yes;
7728  }
7729  else
7730  {
7731  _ui->actionAuto_screen_capture->setChecked(false);
7732  }
7733 
7734  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);
7735  if(r == QMessageBox::No || r == QMessageBox::Yes)
7736  {
7737  _autoScreenCapturePNG = r == QMessageBox::No;
7738  }
7739  else
7740  {
7741  _ui->actionAuto_screen_capture->setChecked(false);
7742  }
7743  }
7744  }
7745  else
7746  {
7747  _ui->actionAuto_screen_capture->setChecked(false);
7748  }
7749  }
7750  else if(_autoScreenCaptureCachedImages.size())
7751  {
7752  QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
7753  QDir dir;
7754  if(!dir.exists(targetDir))
7755  {
7756  dir.mkdir(targetDir);
7757  }
7758  targetDir += QDir::separator();
7759  targetDir += "Main_window";
7760  if(!dir.exists(targetDir))
7761  {
7762  dir.mkdir(targetDir);
7763  }
7764  targetDir += QDir::separator();
7765 
7766  _progressDialog->setCancelButtonVisible(true);
7767  _progressDialog->resetProgress();
7768  _progressDialog->show();
7769  _progressDialog->setMaximumSteps(_autoScreenCaptureCachedImages.size());
7770  int i=0;
7771  for(QMap<QString, QByteArray>::iterator iter=_autoScreenCaptureCachedImages.begin(); iter!=_autoScreenCaptureCachedImages.end() && !_progressDialog->isCanceled(); ++iter)
7772  {
7773  QPixmap figure;
7774  figure.loadFromData(iter.value(), _autoScreenCapturePNG?"PNG":"JPEG");
7775  figure.save(targetDir + iter.key(), _autoScreenCapturePNG?"PNG":"JPEG");
7776  _progressDialog->appendText(tr("Saved image \"%1\" (%2/%3).").arg(targetDir + iter.key()).arg(++i).arg(_autoScreenCaptureCachedImages.size()));
7777  _progressDialog->incrementStep();
7778  QApplication::processEvents();
7779  }
7780  _autoScreenCaptureCachedImages.clear();
7781  _progressDialog->setValue(_progressDialog->maximumSteps());
7782  _progressDialog->setCancelButtonVisible(false);
7783  }
7784 }
7785 
7786 void MainWindow::takeScreenshot()
7787 {
7788  QDesktopServices::openUrl(QUrl::fromLocalFile(this->captureScreen()));
7789 }
7790 
7791 void MainWindow::setAspectRatio(int w, int h)
7792 {
7793  QRect rect = this->geometry();
7794  if(h<100 && w<100)
7795  {
7796  // it is a ratio
7797  if(float(rect.width())/float(rect.height()) > float(w)/float(h))
7798  {
7799  rect.setWidth(w*(rect.height()/h));
7800  rect.setHeight((rect.height()/h)*h);
7801  }
7802  else
7803  {
7804  rect.setHeight(h*(rect.width()/w));
7805  rect.setWidth((rect.width()/w)*w);
7806  }
7807  }
7808  else
7809  {
7810  // it is absolute size
7811  rect.setWidth(w);
7812  rect.setHeight(h);
7813  }
7814  this->setGeometry(rect);
7815 }
7816 
7817 void MainWindow::setAspectRatio16_9()
7818 {
7819  this->setAspectRatio(16, 9);
7820 }
7821 
7822 void MainWindow::setAspectRatio16_10()
7823 {
7824  this->setAspectRatio(16, 10);
7825 }
7826 
7827 void MainWindow::setAspectRatio4_3()
7828 {
7829  this->setAspectRatio(4, 3);
7830 }
7831 
7832 void MainWindow::setAspectRatio240p()
7833 {
7834  this->setAspectRatio((240*16)/9, 240);
7835 }
7836 
7837 void MainWindow::setAspectRatio360p()
7838 {
7839  this->setAspectRatio((360*16)/9, 360);
7840 }
7841 
7842 void MainWindow::setAspectRatio480p()
7843 {
7844  this->setAspectRatio((480*16)/9, 480);
7845 }
7846 
7847 void MainWindow::setAspectRatio720p()
7848 {
7849  this->setAspectRatio((720*16)/9, 720);
7850 }
7851 
7852 void MainWindow::setAspectRatio1080p()
7853 {
7854  this->setAspectRatio((1080*16)/9, 1080);
7855 }
7856 
7857 void MainWindow::setAspectRatioCustom()
7858 {
7859  bool ok;
7860  int width = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Width (pixels):"), this->geometry().width(), 100, 10000, 100, &ok);
7861  if(ok)
7862  {
7863  int height = QInputDialog::getInt(this, tr("Aspect ratio"), tr("Height (pixels):"), this->geometry().height(), 100, 10000, 100, &ok);
7864  if(ok)
7865  {
7866  this->setAspectRatio(width, height);
7867  }
7868  }
7869 }
7870 
7871 void MainWindow::exportGridMap()
7872 {
7873  float gridCellSize = 0.05f;
7874  bool ok;
7875  gridCellSize = (float)QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), (double)gridCellSize, 0.01, 1, 2, &ok);
7876  if(!ok)
7877  {
7878  return;
7879  }
7880 
7881  // create the map
7882  float xMin=0.0f, yMin=0.0f;
7883  cv::Mat pixels;
7884 #ifdef RTABMAP_OCTOMAP
7885  if(_preferencesDialog->isOctomap2dGrid())
7886  {
7887  pixels = _octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
7888 
7889  }
7890  else
7891 #endif
7892  {
7893  pixels = _occupancyGrid->getMap(xMin, yMin);
7894  }
7895 
7896  if(!pixels.empty())
7897  {
7898  cv::Mat map8U(pixels.rows, pixels.cols, CV_8U);
7899  //convert to gray scaled map
7900  for (int i = 0; i < pixels.rows; ++i)
7901  {
7902  for (int j = 0; j < pixels.cols; ++j)
7903  {
7904  char v = pixels.at<char>(i, j);
7905  unsigned char gray;
7906  if(v == 0)
7907  {
7908  gray = 178;
7909  }
7910  else if(v == 100)
7911  {
7912  gray = 0;
7913  }
7914  else // -1
7915  {
7916  gray = 89;
7917  }
7918  map8U.at<unsigned char>(i, j) = gray;
7919  }
7920  }
7921 
7922  QImage image = uCvMat2QImage(map8U, false);
7923 
7924  QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
7925  if(!path.isEmpty())
7926  {
7927  if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
7928  {
7929  //use png by default
7930  path += ".png";
7931  }
7932 
7933  QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
7934  QPixmap::fromImage(img).save(path);
7935 
7936  QDesktopServices::openUrl(QUrl::fromLocalFile(path));
7937  }
7938  }
7939 }
7940 
7941 void MainWindow::exportClouds()
7942 {
7943  if(_exportCloudsDialog->isVisible())
7944  {
7945  return;
7946  }
7947 
7948  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
7949 
7950  // Use ground truth poses if current clouds are using them
7951  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
7952  {
7953  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
7954  {
7955  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
7956  if(gtIter!=_currentGTPosesMap.end())
7957  {
7958  iter->second = gtIter->second;
7959  }
7960  else
7961  {
7962  UWARN("Not found ground truth pose for node %d", iter->first);
7963  }
7964  }
7965  }
7966 
7967  _exportCloudsDialog->exportClouds(
7968  poses,
7969  _currentLinksMap,
7970  _currentMapIds,
7971  _cachedSignatures,
7972  _cachedClouds,
7973  _createdScans,
7974  _preferencesDialog->getWorkingDirectory(),
7975  _preferencesDialog->getAllParameters());
7976 }
7977 
7978 void MainWindow::viewClouds()
7979 {
7980  if(_exportCloudsDialog->isVisible())
7981  {
7982  return;
7983  }
7984 
7985  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
7986 
7987  // Use ground truth poses if current clouds are using them
7988  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
7989  {
7990  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
7991  {
7992  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
7993  if(gtIter!=_currentGTPosesMap.end())
7994  {
7995  iter->second = gtIter->second;
7996  }
7997  else
7998  {
7999  UWARN("Not found ground truth pose for node %d", iter->first);
8000  }
8001  }
8002  }
8003 
8004  _exportCloudsDialog->viewClouds(
8005  poses,
8006  _currentLinksMap,
8007  _currentMapIds,
8008  _cachedSignatures,
8009  _cachedClouds,
8010  _createdScans,
8011  _preferencesDialog->getWorkingDirectory(),
8012  _preferencesDialog->getAllParameters());
8013 
8014 }
8015 
8016 void MainWindow::exportOctomap()
8017 {
8018 #ifdef RTABMAP_OCTOMAP
8019  if(_octomap->octree()->size())
8020  {
8021  QString path = QFileDialog::getSaveFileName(
8022  this,
8023  tr("Save File"),
8024  this->getWorkingDirectory()+"/"+"octomap.bt",
8025  tr("Octomap file (*.bt)"));
8026 
8027  if(!path.isEmpty())
8028  {
8029  if(_octomap->writeBinary(path.toStdString()))
8030  {
8031  QMessageBox::information(this,
8032  tr("Export octomap..."),
8033  tr("Octomap successfully saved to \"%1\".")
8034  .arg(path));
8035  }
8036  else
8037  {
8038  QMessageBox::information(this,
8039  tr("Export octomap..."),
8040  tr("Failed to save octomap to \"%1\"!")
8041  .arg(path));
8042  }
8043  }
8044  }
8045  else
8046  {
8047  UERROR("Empty octomap.");
8048  }
8049 #else
8050  UERROR("Cannot export octomap, RTAB-Map is not built with it.");
8051 #endif
8052 }
8053 
8054 void MainWindow::exportImages()
8055 {
8056  if(_cachedSignatures.empty())
8057  {
8058  QMessageBox::warning(this, tr("Export images..."), tr("Cannot export images, the cache is empty!"));
8059  return;
8060  }
8061  std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
8062 
8063  if(poses.empty())
8064  {
8065  QMessageBox::warning(this, tr("Export images..."), tr("There is no map!"));
8066  return;
8067  }
8068 
8069  QStringList formats;
8070  formats.push_back("id.jpg");
8071  formats.push_back("id.png");
8072  formats.push_back("timestamp.jpg");
8073  formats.push_back("timestamp.png");
8074  bool ok;
8075  QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
8076  if(!ok)
8077  {
8078  return;
8079  }
8080  QString ext = format.split('.').back();
8081  bool useStamp = format.split('.').front().compare("timestamp") == 0;
8082 
8083  QMap<int, double> stamps;
8084  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), this->getWorkingDirectory());
8085  if(!path.isEmpty())
8086  {
8087  SensorData data;
8088  if(_cachedSignatures.contains(poses.rbegin()->first))
8089  {
8090  data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
8091  data.uncompressData();
8092  }
8093 
8094  _progressDialog->resetProgress();
8095  _progressDialog->show();
8096  _progressDialog->setMaximumSteps(_cachedSignatures.size());
8097 
8098  unsigned int saved = 0;
8099  bool calibrationSaved = false;
8100  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
8101  {
8102  QString id = QString::number(iter->first);
8103 
8104  SensorData data;
8105  if(_cachedSignatures.contains(iter->first))
8106  {
8107  data = _cachedSignatures.value(iter->first).sensorData();
8108  data.uncompressData();
8109 
8110  if(!calibrationSaved)
8111  {
8112  if(!data.imageRaw().empty() && !data.rightRaw().empty())
8113  {
8114  QDir dir;
8115  dir.mkdir(QString("%1/left").arg(path));
8116  dir.mkdir(QString("%1/right").arg(path));
8117  if(data.stereoCameraModels().size() > 1)
8118  {
8119  UERROR("Only one stereo camera calibration can be saved at this time (%d detected)", (int)data.stereoCameraModels().size());
8120  }
8121  else if(data.stereoCameraModels().size() == 1 && data.stereoCameraModels().front().isValidForProjection())
8122  {
8123  std::string cameraName = "calibration";
8124  StereoCameraModel model(
8125  cameraName,
8126  data.imageRaw().size(),
8127  data.stereoCameraModels()[0].left().K(),
8128  data.stereoCameraModels()[0].left().D(),
8129  data.stereoCameraModels()[0].left().R(),
8130  data.stereoCameraModels()[0].left().P(),
8131  data.rightRaw().size(),
8132  data.stereoCameraModels()[0].right().K(),
8133  data.stereoCameraModels()[0].right().D(),
8134  data.stereoCameraModels()[0].right().R(),
8135  data.stereoCameraModels()[0].right().P(),
8136  data.stereoCameraModels()[0].R(),
8137  data.stereoCameraModels()[0].T(),
8138  data.stereoCameraModels()[0].E(),
8139  data.stereoCameraModels()[0].F(),
8140  data.stereoCameraModels()[0].left().localTransform());
8141  if(model.save(path.toStdString()))
8142  {
8143  calibrationSaved = true;
8144  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
8145  }
8146  else
8147  {
8148  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
8149  }
8150  }
8151  }
8152  else if(!data.imageRaw().empty())
8153  {
8154  if(!data.depthRaw().empty())
8155  {
8156  QDir dir;
8157  dir.mkdir(QString("%1/rgb").arg(path));
8158  dir.mkdir(QString("%1/depth").arg(path));
8159  }
8160 
8161  if(data.cameraModels().size() > 1)
8162  {
8163  UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
8164  }
8165  else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
8166  {
8167  std::string cameraName = "calibration";
8168  CameraModel model(cameraName,
8169  data.imageRaw().size(),
8170  data.cameraModels().front().K(),
8171  data.cameraModels().front().D(),
8172  data.cameraModels().front().R(),
8173  data.cameraModels().front().P(),
8174  data.cameraModels().front().localTransform());
8175  if(model.save(path.toStdString()))
8176  {
8177  calibrationSaved = true;
8178  UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
8179  }
8180  else
8181  {
8182  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
8183  }
8184  }
8185  }
8186  }
8187 
8188  if(!data.imageRaw().empty() && useStamp)
8189  {
8190  double stamp = _cachedSignatures.value(iter->first).getStamp();
8191  if(stamp == 0.0)
8192  {
8193  UWARN("Node %d has null timestamp! Using id instead!", iter->first);
8194  }
8195  else
8196  {
8197  id = QString::number(stamp, 'f');
8198  }
8199  }
8200  }
8201  QString info;
8202  bool warn = false;
8203  if(!data.imageRaw().empty() && !data.rightRaw().empty())
8204  {
8205  if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
8206  UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
8207  if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
8208  UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
8209  info = tr("Saved left/%1.%2 and right/%1.%2.").arg(id).arg(ext);
8210  }
8211  else if(!data.imageRaw().empty() && !data.depthRaw().empty())
8212  {
8213  if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
8214  UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
8215  if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
8216  UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
8217  info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
8218  }
8219  else if(!data.imageRaw().empty())
8220  {
8221  if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
8222  UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
8223  else
8224  info = tr("Saved %1.%2.").arg(id).arg(ext);
8225  }
8226  else
8227  {
8228  info = tr("No images saved for node %1!").arg(id);
8229  warn = true;
8230  }
8231  saved += warn?0:1;
8232  _progressDialog->appendText(info, !warn?Qt::black:Qt::darkYellow);
8233  _progressDialog->incrementStep();
8234  QApplication::processEvents();
8235 
8236  }
8237  if(saved!=poses.size())
8238  {
8239  _progressDialog->setAutoClose(false);
8240  _progressDialog->appendText(tr("%1 images of %2 saved to \"%3\".").arg(saved).arg(poses.size()).arg(path));
8241  }
8242  else
8243  {
8244  _progressDialog->appendText(tr("%1 images saved to \"%2\".").arg(saved).arg(path));
8245  }
8246 
8247  if(!calibrationSaved)
8248  {
8249  QMessageBox::warning(this,
8250  tr("Export images..."),
8251  tr("Data in the cache don't seem to have valid calibration. Calibration file will not be saved. Try refreshing the cache (with clouds)."));
8252  }
8253 
8254  _progressDialog->setValue(_progressDialog->maximumSteps());
8255  }
8256 }
8257 
8258 void MainWindow::exportBundlerFormat()
8259 {
8260  if(_exportBundlerDialog->isVisible())
8261  {
8262  return;
8263  }
8264 
8265  std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
8266 
8267  // Use ground truth poses if current clouds are using them
8268  if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
8269  {
8270  for(std::map<int, Transform>::iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
8271  {
8272  std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
8273  if(gtIter!=_currentGTPosesMap.end())
8274  {
8275  iter->second = gtIter->second;
8276  }
8277  else
8278  {
8279  UWARN("Not found ground truth pose for node %d", iter->first);
8280  }
8281  }
8282  }
8283 
8284  std::map<int, Transform> poses;
8285  for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
8286  {
8287  if(_cachedSignatures.contains(iter->first))
8288  {
8289  if(_cachedSignatures[iter->first].sensorData().imageRaw().empty() &&
8290  _cachedSignatures[iter->first].sensorData().imageCompressed().empty())
8291  {
8292  UWARN("Missing image in cache for node %d", iter->first);
8293  }
8294  else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
8295  _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
8296  (_cachedSignatures[iter->first].sensorData().stereoCameraModels().size() == 1 &&
8297  _cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].isValidForProjection()))
8298  {
8299  poses.insert(*iter);
8300  }
8301  else
8302  {
8303  UWARN("Missing calibration for node %d", iter->first);
8304  }
8305  }
8306  else
8307  {
8308  UWARN("Did not find node %d in cache", iter->first);
8309  }
8310  }
8311 
8312  if(poses.size())
8313  {
8314  _exportBundlerDialog->exportBundler(
8315  poses,
8316  _currentLinksMap,
8317  _cachedSignatures,
8318  _preferencesDialog->getAllParameters());
8319  }
8320  else
8321  {
8322  QMessageBox::warning(this, tr("Exporting cameras..."), tr("No poses exported because of missing images. Try refreshing the cache (with clouds)."));
8323  }
8324 }
8325 
8326 void MainWindow::resetOdometry()
8327 {
8328  UINFO("reset odometry");
8329  this->post(new OdometryResetEvent());
8330 }
8331 
8332 void MainWindow::triggerNewMap()
8333 {
8334  UINFO("trigger a new map");
8335  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap));
8336 }
8337 
8338 void MainWindow::dataRecorder()
8339 {
8340  if(_dataRecorder == 0)
8341  {
8342  QString path = QFileDialog::getSaveFileName(this, tr("Save to..."), _preferencesDialog->getWorkingDirectory()+"/output.db", "RTAB-Map database (*.db)");
8343  if(!path.isEmpty())
8344  {
8345  int r = QMessageBox::question(this, tr("Hard drive or RAM?"), tr("Save in RAM?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
8346 
8347  if(r == QMessageBox::No || r == QMessageBox::Yes)
8348  {
8349  bool recordInRAM = r == QMessageBox::Yes;
8350 
8351  _dataRecorder = new DataRecorder(this);
8352  _dataRecorder->setWindowFlags(Qt::Dialog);
8353  _dataRecorder->setAttribute(Qt::WA_DeleteOnClose, true);
8354  _dataRecorder->setWindowTitle(tr("Data recorder (%1)").arg(path));
8355 
8356  if(_dataRecorder->init(path, recordInRAM))
8357  {
8358  this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed()));
8359  _dataRecorder->show();
8360  _dataRecorder->registerToEventsManager();
8361  if(_sensorCapture)
8362  {
8363  UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent");
8364  }
8365  _ui->actionData_recorder->setEnabled(false);
8366  }
8367  else
8368  {
8369  QMessageBox::warning(this, tr(""), tr("Cannot initialize the data recorder!"));
8370  UERROR("Cannot initialize the data recorder!");
8371  delete _dataRecorder;
8372  _dataRecorder = 0;
8373  }
8374  }
8375  }
8376  }
8377  else
8378  {
8379  UERROR("Only one recorder at the same time.");
8380  }
8381 }
8382 
8383 void MainWindow::dataRecorderDestroyed()
8384 {
8385  _ui->actionData_recorder->setEnabled(true);
8386  _dataRecorder = 0;
8387 }
8388 
8389 //END ACTIONS
8390 
8391 // STATES
8392 
8393 // in monitoring state, only some actions are enabled
8394 void MainWindow::setMonitoringState(bool pauseChecked)
8395 {
8396  this->changeState(pauseChecked?kMonitoringPaused:kMonitoring);
8397 }
8398 
8399 // Must be called by the GUI thread, use signal StateChanged()
8400 void MainWindow::changeState(MainWindow::State newState)
8401 {
8402  bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
8403  _ui->label_source->setVisible(!monitoring);
8404  _ui->label_stats_source->setVisible(!monitoring);
8405  _ui->actionNew_database->setVisible(!monitoring);
8406  _ui->actionOpen_database->setVisible(!monitoring);
8407  _ui->actionClose_database->setVisible(!monitoring);
8408  _ui->actionEdit_database->setVisible(!monitoring);
8409  _ui->actionStart->setVisible(!monitoring);
8410  _ui->actionStop->setVisible(!monitoring);
8411  _ui->actionDump_the_memory->setVisible(!monitoring);
8412  _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
8413  _ui->actionGenerate_map->setVisible(!monitoring);
8414  _ui->actionUpdate_cache_from_database->setVisible(monitoring);
8415  _ui->actionData_recorder->setVisible(!monitoring);
8416  _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
8417  _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
8418  _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
8419  bool wasMonitoring = _state==kMonitoring || _state == kMonitoringPaused;
8420  if(wasMonitoring != monitoring)
8421  {
8422  _ui->toolBar->setVisible(!monitoring);
8423  _ui->toolBar->toggleViewAction()->setVisible(!monitoring);
8424  }
8425  QList<QAction*> actions = _ui->menuTools->actions();
8426  for(int i=0; i<actions.size(); ++i)
8427  {
8428  if(actions.at(i)->isSeparator())
8429  {
8430  actions.at(i)->setVisible(!monitoring);
8431  }
8432  }
8433  actions = _ui->menuFile->actions();
8434  if(actions.size()==16)
8435  {
8436  if(actions.at(2)->isSeparator())
8437  {
8438  actions.at(2)->setVisible(!monitoring);
8439  }
8440  else
8441  {
8442  UWARN("Menu File separators have not the same order.");
8443  }
8444  if(actions.at(12)->isSeparator())
8445  {
8446  actions.at(12)->setVisible(!monitoring);
8447  }
8448  else
8449  {
8450  UWARN("Menu File separators have not the same order.");
8451  }
8452  }
8453  else
8454  {
8455  UWARN("Menu File actions size has changed (%d)", actions.size());
8456  }
8457  actions = _ui->menuProcess->actions();
8458  if(actions.size()>=2)
8459  {
8460  if(actions.at(1)->isSeparator())
8461  {
8462  actions.at(1)->setVisible(!monitoring);
8463  }
8464  else
8465  {
8466  UWARN("Menu File separators have not the same order.");
8467  }
8468  }
8469  else
8470  {
8471  UWARN("Menu File separators have not the same order.");
8472  }
8473 
8474  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
8475 
8476  switch (newState)
8477  {
8478  case kIdle: // RTAB-Map is not initialized yet
8479  _ui->actionNew_database->setEnabled(true);
8480  _ui->actionOpen_database->setEnabled(true);
8481  _ui->actionClose_database->setEnabled(false);
8482  _ui->actionEdit_database->setEnabled(true);
8483  _ui->actionStart->setEnabled(false);
8484  _ui->actionPause->setEnabled(false);
8485  _ui->actionPause->setChecked(false);
8486  _ui->actionPause->setToolTip(tr("Pause"));
8487  _ui->actionStop->setEnabled(false);
8488  _ui->actionPause_on_match->setEnabled(true);
8489  _ui->actionPause_on_local_loop_detection->setEnabled(true);
8490  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
8491  _ui->actionDump_the_memory->setEnabled(false);
8492  _ui->actionDump_the_prediction_matrix->setEnabled(false);
8493  _ui->actionDelete_memory->setEnabled(false);
8494  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
8495  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8496  _ui->actionGenerate_map->setEnabled(false);
8497  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
8498  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8499  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8500  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
8501 #ifdef RTABMAP_OCTOMAP
8502  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
8503 #else
8504  _ui->actionExport_octomap->setEnabled(false);
8505 #endif
8506  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8507  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8508  _ui->actionDownload_all_clouds->setEnabled(false);
8509  _ui->actionDownload_graph->setEnabled(false);
8510  _ui->menuSelect_source->setEnabled(true);
8511  _ui->actionLabel_current_location->setEnabled(false);
8512  _ui->actionSend_goal->setEnabled(false);
8513  _ui->actionCancel_goal->setEnabled(false);
8514  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
8515  _ui->actionTrigger_a_new_map->setEnabled(false);
8516  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
8517  _ui->statusbar->clearMessage();
8518  _state = newState;
8519  _oneSecondTimer->stop();
8520  break;
8521 
8522  case kApplicationClosing:
8523  case kClosing:
8524  _ui->actionStart->setEnabled(false);
8525  _ui->actionPause->setEnabled(false);
8526  _ui->actionStop->setEnabled(false);
8527  _state = newState;
8528  break;
8529 
8530  case kInitializing:
8531  _ui->actionNew_database->setEnabled(false);
8532  _ui->actionOpen_database->setEnabled(false);
8533  _ui->actionClose_database->setEnabled(false);
8534  _ui->actionEdit_database->setEnabled(false);
8535  _state = newState;
8536  break;
8537 
8538  case kInitialized:
8539  _ui->actionNew_database->setEnabled(false);
8540  _ui->actionOpen_database->setEnabled(false);
8541  _ui->actionClose_database->setEnabled(true);
8542  _ui->actionEdit_database->setEnabled(false);
8543  _ui->actionStart->setEnabled(true);
8544  _ui->actionPause->setEnabled(false);
8545  _ui->actionPause->setChecked(false);
8546  _ui->actionPause->setToolTip(tr("Pause"));
8547  _ui->actionStop->setEnabled(false);
8548  _ui->actionPause_on_match->setEnabled(true);
8549  _ui->actionPause_on_local_loop_detection->setEnabled(true);
8550  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
8551  _ui->actionDump_the_memory->setEnabled(true);
8552  _ui->actionDump_the_prediction_matrix->setEnabled(true);
8553  _ui->actionDelete_memory->setEnabled(_openedDatabasePath.isEmpty());
8554  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
8555  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8556  _ui->actionGenerate_map->setEnabled(true);
8557  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
8558  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8559  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8560  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
8561 #ifdef RTABMAP_OCTOMAP
8562  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
8563 #else
8564  _ui->actionExport_octomap->setEnabled(false);
8565 #endif
8566  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8567  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8568  _ui->actionDownload_all_clouds->setEnabled(true);
8569  _ui->actionDownload_graph->setEnabled(true);
8570  _ui->menuSelect_source->setEnabled(true);
8571  _ui->actionLabel_current_location->setEnabled(true);
8572  _ui->actionSend_goal->setEnabled(true);
8573  _ui->actionCancel_goal->setEnabled(true);
8574  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
8575  _ui->actionTrigger_a_new_map->setEnabled(true);
8576  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
8577  _ui->statusbar->clearMessage();
8578  _state = newState;
8579  _oneSecondTimer->stop();
8580  break;
8581 
8582  case kStartingDetection:
8583  _ui->actionStart->setEnabled(false);
8584  _state = newState;
8585  break;
8586 
8587  case kDetecting:
8588  _ui->actionNew_database->setEnabled(false);
8589  _ui->actionOpen_database->setEnabled(false);
8590  _ui->actionClose_database->setEnabled(false);
8591  _ui->actionEdit_database->setEnabled(false);
8592  _ui->actionStart->setEnabled(false);
8593  _ui->actionPause->setEnabled(true);
8594  _ui->actionPause->setChecked(false);
8595  _ui->actionPause->setToolTip(tr("Pause"));
8596  _ui->actionStop->setEnabled(true);
8597  _ui->actionPause_on_match->setEnabled(true);
8598  _ui->actionPause_on_local_loop_detection->setEnabled(true);
8599  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
8600  _ui->actionDump_the_memory->setEnabled(false);
8601  _ui->actionDump_the_prediction_matrix->setEnabled(false);
8602  _ui->actionDelete_memory->setEnabled(false);
8603  _ui->actionPost_processing->setEnabled(false);
8604  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
8605  _ui->actionGenerate_map->setEnabled(false);
8606  _ui->menuExport_poses->setEnabled(false);
8607  _ui->actionSave_point_cloud->setEnabled(false);
8608  _ui->actionView_high_res_point_cloud->setEnabled(false);
8609  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
8610  _ui->actionExport_octomap->setEnabled(false);
8611  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
8612  _ui->actionDepth_Calibration->setEnabled(false);
8613  _ui->actionDownload_all_clouds->setEnabled(false);
8614  _ui->actionDownload_graph->setEnabled(false);
8615  _ui->menuSelect_source->setEnabled(false);
8616  _ui->actionLabel_current_location->setEnabled(true);
8617  _ui->actionSend_goal->setEnabled(true);
8618  _ui->actionCancel_goal->setEnabled(true);
8619  _ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
8620  _ui->actionTrigger_a_new_map->setEnabled(true);
8621  _ui->doubleSpinBox_stats_imgRate->setEnabled(true);
8622  _ui->statusbar->showMessage(tr("Detecting..."));
8623  _state = newState;
8624  _ui->label_elapsedTime->setText("00:00:00");
8625  _elapsedTime->start();
8626  _oneSecondTimer->start();
8627 
8628  _databaseUpdated = true; // if a new database is used, it won't be empty anymore...
8629 
8630  if(_sensorCapture)
8631  {
8632  _sensorCapture->start();
8633  if(_imuThread)
8634  {
8635  _imuThread->start();
8636  }
8637  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
8638  }
8639  break;
8640 
8641  case kPaused:
8642  if(_state == kPaused)
8643  {
8644  _ui->actionPause->setToolTip(tr("Pause"));
8645  _ui->actionPause->setChecked(false);
8646  _ui->statusbar->showMessage(tr("Detecting..."));
8647  _ui->actionDump_the_memory->setEnabled(false);
8648  _ui->actionDump_the_prediction_matrix->setEnabled(false);
8649  _ui->actionDelete_memory->setEnabled(false);
8650  _ui->actionPost_processing->setEnabled(false);
8651  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
8652  _ui->actionGenerate_map->setEnabled(false);
8653  _ui->menuExport_poses->setEnabled(false);
8654  _ui->actionSave_point_cloud->setEnabled(false);
8655  _ui->actionView_high_res_point_cloud->setEnabled(false);
8656  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
8657  _ui->actionExport_octomap->setEnabled(false);
8658  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
8659  _ui->actionDepth_Calibration->setEnabled(false);
8660  _ui->actionDownload_all_clouds->setEnabled(false);
8661  _ui->actionDownload_graph->setEnabled(false);
8662  _state = kDetecting;
8663  _elapsedTime->start();
8664  _oneSecondTimer->start();
8665 
8666  if(_sensorCapture)
8667  {
8668  _sensorCapture->start();
8669  if(_imuThread)
8670  {
8671  _imuThread->start();
8672  }
8673  ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
8674  }
8675  }
8676  else if(_state == kDetecting)
8677  {
8678  _ui->actionPause->setToolTip(tr("Continue (shift-click for step-by-step)"));
8679  _ui->actionPause->setChecked(true);
8680  _ui->statusbar->showMessage(tr("Paused..."));
8681  _ui->actionDump_the_memory->setEnabled(true);
8682  _ui->actionDump_the_prediction_matrix->setEnabled(true);
8683  _ui->actionDelete_memory->setEnabled(false);
8684  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
8685  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8686  _ui->actionGenerate_map->setEnabled(true);
8687  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
8688  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8689  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8690  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
8691 #ifdef RTABMAP_OCTOMAP
8692  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
8693 #else
8694  _ui->actionExport_octomap->setEnabled(false);
8695 #endif
8696  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8697  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8698  _ui->actionDownload_all_clouds->setEnabled(true);
8699  _ui->actionDownload_graph->setEnabled(true);
8700  _state = kPaused;
8701  _oneSecondTimer->stop();
8702 
8703  // kill sensors
8704  if(_sensorCapture)
8705  {
8706  if(_imuThread)
8707  {
8708  _imuThread->join(true);
8709  }
8710  _sensorCapture->join(true);
8711  }
8712  }
8713  break;
8714  case kMonitoring:
8715  _ui->actionPause->setEnabled(true);
8716  _ui->actionPause->setChecked(false);
8717  _ui->actionPause->setToolTip(tr("Pause"));
8718  _ui->actionPause_on_match->setEnabled(true);
8719  _ui->actionPause_on_local_loop_detection->setEnabled(true);
8720  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
8721  _ui->actionReset_Odometry->setEnabled(true);
8722  _ui->actionPost_processing->setEnabled(false);
8723  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
8724  _ui->menuExport_poses->setEnabled(false);
8725  _ui->actionSave_point_cloud->setEnabled(false);
8726  _ui->actionView_high_res_point_cloud->setEnabled(false);
8727  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
8728  _ui->actionExport_octomap->setEnabled(false);
8729  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
8730  _ui->actionDepth_Calibration->setEnabled(false);
8731  _ui->actionDelete_memory->setEnabled(true);
8732  _ui->actionDownload_all_clouds->setEnabled(true);
8733  _ui->actionDownload_graph->setEnabled(true);
8734  _ui->actionTrigger_a_new_map->setEnabled(true);
8735  _ui->actionLabel_current_location->setEnabled(true);
8736  _ui->actionSend_goal->setEnabled(true);
8737  _ui->actionCancel_goal->setEnabled(true);
8738  _ui->statusbar->showMessage(tr("Monitoring..."));
8739  _state = newState;
8740  _elapsedTime->start();
8741  _oneSecondTimer->start();
8742  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResume));
8743  break;
8744  case kMonitoringPaused:
8745  _ui->actionPause->setToolTip(tr("Continue"));
8746  _ui->actionPause->setChecked(true);
8747  _ui->actionPause->setEnabled(true);
8748  _ui->actionPause_on_match->setEnabled(true);
8749  _ui->actionPause_on_local_loop_detection->setEnabled(true);
8750  _ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
8751  _ui->actionReset_Odometry->setEnabled(true);
8752  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
8753  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8754  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
8755  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8756  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
8757  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
8758 #ifdef RTABMAP_OCTOMAP
8759  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
8760 #else
8761  _ui->actionExport_octomap->setEnabled(false);
8762 #endif
8763  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8764  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
8765  _ui->actionDelete_memory->setEnabled(true);
8766  _ui->actionDownload_all_clouds->setEnabled(true);
8767  _ui->actionDownload_graph->setEnabled(true);
8768  _ui->actionTrigger_a_new_map->setEnabled(true);
8769  _ui->actionLabel_current_location->setEnabled(true);
8770  _ui->actionSend_goal->setEnabled(true);
8771  _ui->actionCancel_goal->setEnabled(true);
8772  _ui->statusbar->showMessage(tr("Monitoring paused..."));
8773  _state = newState;
8774  _oneSecondTimer->stop();
8775  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause));
8776  break;
8777  default:
8778  break;
8779  }
8780 
8781 }
8782 
8783 }
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:133
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::MainWindow::alignPosesToGroundTruth
Transform alignPosesToGroundTruth(const std::map< int, Transform > &poses, const std::map< int, Transform > &groundTruth)
Definition: MainWindow.cpp:4411
rtabmap::PreferencesDialog::beepOnPause
bool beepOnPause() const
Definition: PreferencesDialog.cpp:5775
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
rtabmap::PreferencesDialog::isVerticalLayoutUsed
bool isVerticalLayoutUsed() const
Definition: PreferencesDialog.cpp:5763
rtabmap::MainWindow::_processingDownloadedMap
bool _processingDownloadedMap
Definition: MainWindow.h:361
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
w
RowVector3d w
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::LoopClosureViewer::updateView
void updateView(const Transform &AtoB=Transform(), const ParametersMap &parameters=ParametersMap())
Definition: LoopClosureViewer.cpp:70
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
rtabmap::RtabmapGlobalPathEvent::getGoal
int getGoal() const
Definition: RtabmapEvent.h:225
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::MainWindow::setMonitoringState
void setMonitoringState(bool pauseChecked=false)
Definition: MainWindow.cpp:8394
rtabmap::Statistics::loopClosureId
int loopClosureId() const
Definition: Statistics.h:266
rtabmap::CloudViewer::removeAllCoordinates
void removeAllCoordinates(const std::string &prefix="")
Definition: CloudViewer.cpp:1797
compare
bool compare
rtabmap::PreferencesDialog::isCloudFiltering
bool isCloudFiltering() const
Definition: PreferencesDialog.cpp:6088
rtabmap::DatabaseViewer
Definition: DatabaseViewer.h:68
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1197
rtabmap::MainWindow::_waypointsIndex
int _waypointsIndex
Definition: MainWindow.h:373
rtabmap::MainWindow::_cachedLocalizationsCount
std::map< int, float > _cachedLocalizationsCount
Definition: MainWindow.h:389
rtabmap::PreferencesDialog::getOdomStrategy
int getOdomStrategy() const
Definition: PreferencesDialog.cpp:7063
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2979
rtabmap::MainWindow::changeState
virtual void changeState(MainWindow::State state)
Definition: MainWindow.cpp:8400
rtabmap::ProgressDialog
Definition: ProgressDialog.h:43
rtabmap::MainWindow::updateSelectSourceMenu
void updateSelectSourceMenu()
Definition: MainWindow.cpp:5266
rtabmap::PostProcessingDialog::sbaIterations
int sbaIterations() const
Definition: PostProcessingDialog.cpp:272
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:58
rtabmap::MainWindow::_cachedClouds
std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > _cachedClouds
Definition: MainWindow.h:384
save
save
rtabmap::PostProcessingDialog::clusterRadius
double clusterRadius() const
Definition: PostProcessingDialog.cpp:232
rtabmap::MainWindow::_newDatabasePathOutput
QString _newDatabasePathOutput
Definition: MainWindow.h:365
rtabmap::PreferencesDialog::kPanelLogging
@ kPanelLogging
Definition: PreferencesDialog.h:77
rtabmap::MainWindow::_odomThread
rtabmap::OdometryThread * _odomThread
Definition: MainWindow.h:345
rtabmap::OdometryInfo::localMapSize
int localMapSize
Definition: OdometryInfo.h:101
rtabmap::PreferencesDialog::getCloudColorScheme
int getCloudColorScheme(int index) const
Definition: PreferencesDialog.cpp:6015
rtabmap::MainWindow::openPreferences
virtual void openPreferences()
Definition: MainWindow.cpp:7667
UINFO
#define UINFO(...)
rtabmap::PreferencesDialog::getSourceScanForceGroundNormalsUp
double getSourceScanForceGroundNormalsUp() const
Definition: PreferencesDialog.cpp:6399
rtabmap::PreferencesDialog::loadCustomConfig
QString loadCustomConfig(const QString &section, const QString &key)
Definition: PreferencesDialog.cpp:4116
name
rtabmap::PostProcessingDialog::isSBA
bool isSBA() const
Definition: PostProcessingDialog.cpp:267
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::RegistrationInfo::icpStructuralComplexity
float icpStructuralComplexity
Definition: RegistrationInfo.h:93
rtabmap::PreferencesDialog::getIMUGravityLength
double getIMUGravityLength(int index) const
Definition: PreferencesDialog.cpp:5945
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::RtabmapEventInit::kInitialized
@ kInitialized
Definition: RtabmapEvent.h:141
rtabmap::MainWindow::_rawLikelihoodCurve
PdfPlotCurve * _rawLikelihoodCurve
Definition: MainWindow.h:410
ImageView.h
rtabmap::SensorData::uncompressData
void uncompressData()
Definition: SensorData.cpp:528
rtabmap::Statistics::getLastSignatureData
const Signature & getLastSignatureData() const
Definition: Statistics.h:272
rtabmap::PreferencesDialog::getIMURate
int getIMURate() const
Definition: PreferencesDialog.cpp:6306
rtabmap::PreferencesDialog::isSourceRGBDColorOnly
bool isSourceRGBDColorOnly() const
Definition: PreferencesDialog.cpp:6315
rtabmap::Statistics
Definition: Statistics.h:53
rtabmap::CloudViewer::removeLine
void removeLine(const std::string &id)
Definition: CloudViewer.cpp:1851
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::RtabmapEventCmd
Definition: RtabmapEvent.h:57
rtabmap::PostProcessingDialog::intraSession
bool intraSession() const
Definition: PostProcessingDialog.cpp:247
rtabmap::MainWindow::selectOpenniCvAsus
void selectOpenniCvAsus()
Definition: MainWindow.cpp:7149
rtabmap::ProgressDialog::clear
void clear()
Definition: ProgressDialog.cpp:148
SensorCaptureThread.h
rtabmap::IMUThread::init
bool init(const std::string &path)
Definition: IMUThread.cpp:54
rtabmap::PreferencesDialog::kSrcUsbDevice
@ kSrcUsbDevice
Definition: PreferencesDialog.h:116
rtabmap::RtabmapEventInit
Definition: RtabmapEvent.h:135
ParamEvent.h
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:430
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::RtabmapEventInit::Status
Status
Definition: RtabmapEvent.h:139
util3d_surface.h
rtabmap::MainWindow::selectK4W2
void selectK4W2()
Definition: MainWindow.cpp:7164
bytes
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
rtabmap::SensorCaptureThread::setScanParameters
RTABMAP_DEPRECATED void setScanParameters(bool fromDepth, int downsampleStep, float rangeMin, float rangeMax, float voxelSize, int normalsK, float normalsRadius, bool forceGroundNormalsUp, bool deskewing)
Definition: SensorCaptureThread.cpp:262
rtabmap::MainWindow::rtabmapEventInitReceived
void rtabmapEventInitReceived(int status, const QString &info)
rtabmap::MainWindow::_firstCall
bool _firstCall
Definition: MainWindow.h:430
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::MainWindow::sendGoal
void sendGoal()
Definition: MainWindow.cpp:7261
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::CameraStereoFlyCapture2::available
static bool available()
Definition: CameraStereoFlyCapture2.cpp:68
rtabmap::CloudViewer::removeOccupancyGridMap
void removeOccupancyGridMap()
Definition: CloudViewer.cpp:1595
rtabmap::PreferencesDialog::isRGBDMode
bool isRGBDMode() const
Definition: PreferencesDialog.cpp:7101
rtabmap::PreferencesDialog::isSourceStereoExposureCompensation
bool isSourceStereoExposureCompensation() const
Definition: PreferencesDialog.cpp:6363
rtabmap::OdometryThread
Definition: OdometryThread.h:41
rtabmap::OdometryEvent::pose
const Transform & pose() const
Definition: OdometryEvent.h:71
rtabmap::CloudViewer::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: CloudViewer.cpp:3175
rtabmap::MainWindow::selectK4A
void selectK4A()
Definition: MainWindow.cpp:7169
rtabmap::MainWindow::_odometryReceived
bool _odometryReceived
Definition: MainWindow.h:363
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
rtabmap::MainWindow::resetOdometry
virtual void resetOdometry()
Definition: MainWindow.cpp:8326
rtabmap::LoopClosureViewer::setDecimation
void setDecimation(int decimation)
Definition: LoopClosureViewer.h:57
rtabmap::MainWindow::_oneSecondTimer
QTimer * _oneSecondTimer
Definition: MainWindow.h:404
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
OptimizerCVSBA.h
rtabmap::PreferencesDialog::isSourceFeatureDetection
bool isSourceFeatureDetection() const
Definition: PreferencesDialog.cpp:6355
rtabmap::MainWindow::_createdFeatures
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > _createdFeatures
Definition: MainWindow.h:398
timer
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
s
RealScalar s
rtabmap::OdometryInfo::timeParticleFiltering
float timeParticleFiltering
Definition: OdometryInfo.h:112
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:1074
rtabmap::ParamEvent
Definition: ParamEvent.h:41
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:996
IMUThread.h
rtabmap::CameraFreenect::available
static bool available()
Definition: CameraFreenect.cpp:284
b
Array< int, 3, 1 > b
UEventsHandler::registerToEventsManager
void registerToEventsManager()
Definition: UEventsHandler.cpp:29
rtabmap::MainWindow::changeTimeLimitSetting
void changeTimeLimitSetting()
Definition: MainWindow.cpp:5320
rtabmap::PreferencesDialog::getIMUPath
QString getIMUPath() const
Definition: PreferencesDialog.cpp:6293
rtabmap::PreferencesDialog::isOctomapUpdated
bool isOctomapUpdated() const
Definition: PreferencesDialog.cpp:5841
rtabmap::PreferencesDialog::isCloudMeshingTexture
bool isCloudMeshingTexture() const
Definition: PreferencesDialog.cpp:5974
rtabmap::MainWindow::imgRateChanged
void imgRateChanged(double)
rtabmap::MainWindow::_ui
Ui_mainWindow * _ui
Definition: MainWindow.h:341
rtabmap::MainWindow::_loopClosureViewer
LoopClosureViewer * _loopClosureViewer
Definition: MainWindow.h:417
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:745
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:129
rtabmap::CameraRealSense::available
static bool available()
Definition: CameraRealSense.cpp:47
rtabmap::ExportCloudsDialog
Definition: ExportCloudsDialog.h:54
rtabmap::CameraStereoDC1394::available
static bool available()
Definition: CameraStereoDC1394.cpp:317
keys
const KeyVector keys
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraK4W2.cpp:53
rtabmap::MainWindow::_state
State _state
Definition: MainWindow.h:343
rtabmap::MainWindow::showPostProcessingDialog
void showPostProcessingDialog()
Definition: MainWindow.cpp:6460
rtabmap::OdometryInfo::localBundleTime
float localBundleTime
Definition: OdometryInfo.h:106
rtabmap::RegistrationIcp
Definition: RegistrationIcp.h:39
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::PreferencesDialog::getSubtractFilteringAngle
double getSubtractFilteringAngle() const
Definition: PreferencesDialog.cpp:6112
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_CORE_EXPORT radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1463
rtabmap::PreferencesDialog::isRelocalizationColorOdomCacheGraphView
bool isRelocalizationColorOdomCacheGraphView() const
Definition: PreferencesDialog.cpp:5811
rtabmap::Registration
Definition: Registration.h:39
rtabmap::MainWindow::beep
void beep()
Definition: MainWindow.cpp:5371
rtabmap::RtabmapEventCmd::kCmdPublish3DMap
@ kCmdPublish3DMap
Definition: RtabmapEvent.h:72
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
rtabmap::MainWindow::updateParameters
void updateParameters(const rtabmap::ParametersMap &parameters)
Definition: MainWindow.cpp:5387
rtabmap::MainWindow::_rectCameraModels
std::vector< CameraModel > _rectCameraModels
Definition: MainWindow.h:374
rtabmap::MainWindow::selectRealSense2
void selectRealSense2()
Definition: MainWindow.cpp:7179
rtabmap::PreferencesDialog::getAllParameters
rtabmap::ParametersMap getAllParameters() const
Definition: PreferencesDialog.cpp:4136
LoopClosureViewer.h
rtabmap::MainWindow::drawKeypoints
void drawKeypoints(const std::multimap< int, cv::KeyPoint > &refWords, const std::multimap< int, cv::KeyPoint > &loopWords)
Definition: MainWindow.cpp:4982
UFile::length
long length()
Definition: UFile.h:110
rtabmap::PreferencesDialog::getParameter
std::string getParameter(const std::string &key) const
Definition: PreferencesDialog.cpp:4148
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
rtabmap::Statistics::localPath
const std::vector< int > & localPath() const
Definition: Statistics.h:285
h
const double h
rtabmap::PreferencesDialog::kSrcStereoMyntEye
@ kSrcStereoMyntEye
Definition: PreferencesDialog.h:111
rtabmap::RegistrationInfo::icpRMS
float icpRMS
Definition: RegistrationInfo.h:96
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
rtabmap::RtabmapEventCmd::kCmdGenerateDOTGraph
@ kCmdGenerateDOTGraph
Definition: RtabmapEvent.h:69
rtabmap::MainWindow::rtabmapGlobalPathEventReceived
void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &event)
OdometryThread.h
rtabmap::IMUThread
Definition: IMUThread.h:49
rtabmap::MainWindow::kInitialized
@ kInitialized
Definition: MainWindow.h:90
rtabmap::OdometryInfo::keyFrameAdded
bool keyFrameAdded
Definition: OdometryInfo.h:109
rtabmap::MainWindow::_exportCloudsDialog
ExportCloudsDialog * _exportCloudsDialog
Definition: MainWindow.h:351
rtabmap::OdometryInfo::timeEstimation
float timeEstimation
Definition: OdometryInfo.h:111
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
rtabmap::PreferencesDialog::isFramesShown
bool isFramesShown() const
Definition: PreferencesDialog.cpp:5928
rtabmap::PreferencesDialog::getSourceScanNormalsRadius
double getSourceScanNormalsRadius() const
Definition: PreferencesDialog.cpp:6395
rtabmap::PreferencesDialog::getGeneralLoggerPrintTime
bool getGeneralLoggerPrintTime() const
Definition: PreferencesDialog.cpp:5743
rtabmap::MainWindow::_postProcessingDialog
PostProcessingDialog * _postProcessingDialog
Definition: MainWindow.h:353
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:283
rtabmap::MainWindow::setupMainLayout
void setupMainLayout(bool vertical)
Definition: MainWindow.cpp:740
size
Index size
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:2296
clams::calibrate
DiscreteDepthDistortionModel RTABMAP_CORE_EXPORT 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)
rtabmap::CameraModel
Definition: CameraModel.h:38
uStr2Bool
bool UTILITE_EXPORT uStr2Bool(const char *str)
Definition: UConversion.cpp:170
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::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:989
StatsToolBox.h
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_CORE_EXPORT 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:730
rtabmap::PreferencesDialog::kSrcRGBDImages
@ kSrcRGBDImages
Definition: PreferencesDialog.h:96
rtabmap::OdometryInfo::features
int features
Definition: OdometryInfo.h:100
rtabmap::PreferencesDialog::isGraphsShown
bool isGraphsShown() const
Definition: PreferencesDialog.cpp:5920
rtabmap::MainWindow::cameraInfoProcessed
void cameraInfoProcessed()
rtabmap::MainWindow::odometryReceived
void odometryReceived(const rtabmap::OdometryEvent &, bool)
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT 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::createAndAddCloudToMap
std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > createAndAddCloudToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:3607
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::CloudViewer::updateFrustumPose
bool updateFrustumPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:2225
rtabmap::RegistrationInfo::inliersDistribution
float inliersDistribution
Definition: RegistrationInfo.h:83
rtabmap::MainWindow::_databaseUpdated
bool _databaseUpdated
Definition: MainWindow.h:368
rtabmap::OdometryInfo::transformFiltered
Transform transformFiltered
Definition: OdometryInfo.h:116
rtabmap::MainWindow::exportPosesG2O
void exportPosesG2O()
Definition: MainWindow.cpp:6252
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::RegistrationInfo::icpCorrespondences
int icpCorrespondences
Definition: RegistrationInfo.h:95
rtabmap::PreferencesDialog::getScanNormalRadiusSearch
double getScanNormalRadiusSearch() const
Definition: PreferencesDialog.cpp:5915
rtabmap::PreferencesDialog::isOctomap2dGrid
bool isOctomap2dGrid() const
Definition: PreferencesDialog.cpp:5859
rtabmap::MainWindow::openHelp
virtual void openHelp()
Definition: MainWindow.cpp:7570
rtabmap::PreferencesDialog::isTimeUsedInFigures
bool isTimeUsedInFigures() const
Definition: PreferencesDialog.cpp:5779
rtabmap::PreferencesDialog::isFrustumsShown
bool isFrustumsShown(int index) const
Definition: PreferencesDialog.cpp:6077
rtabmap::CloudViewer::getPose
bool getPose(const std::string &id, Transform &pose)
Definition: CloudViewer.cpp:2604
rtabmap::MainWindow::postGoal
void postGoal()
Definition: MainWindow.cpp:7296
rtabmap::MainWindow::dataRecorderDestroyed
void dataRecorderDestroyed()
Definition: MainWindow.cpp:8383
rtabmap::RtabmapEventCmd::kCmdTriggerNewMap
@ kCmdTriggerNewMap
Definition: RtabmapEvent.h:74
rtabmap::PreferencesDialog::loadMainWindowState
void loadMainWindowState(QMainWindow *mainWindow, bool &maximized, bool &statusBarShown)
Definition: PreferencesDialog.cpp:3919
count
Index count
rtabmap::RtabmapLabelErrorEvent
Definition: RtabmapEvent.h:237
rtabmap::MainWindow::resizeEvent
virtual void resizeEvent(QResizeEvent *anEvent)
Definition: MainWindow.cpp:5236
rtabmap::MainWindow::processRtabmapGoalStatusEvent
void processRtabmapGoalStatusEvent(int status)
Definition: MainWindow.cpp:4857
PostProcessingDialog.h
rtabmap::MainWindow::_multiSessionLocWidget
MultiSessionLocWidget * _multiSessionLocWidget
Definition: MainWindow.h:412
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::SensorCaptureThread::setColorOnly
void setColorOnly(bool colorOnly)
Definition: SensorCaptureThread.h:125
rtabmap::ProgressDialog::resetProgress
void resetProgress()
Definition: ProgressDialog.cpp:155
rtabmap::MainWindow::_autoScreenCapturePNG
bool _autoScreenCapturePNG
Definition: MainWindow.h:424
rtabmap::Signature::mapId
int mapId() const
Definition: Signature.h:71
rtabmap::Signature::getGroundTruthPose
const Transform & getGroundTruthPose() const
Definition: Signature.h:135
rtabmap::MainWindow::label
void label()
Definition: MainWindow.cpp:7331
rtabmap::CloudViewer::removeAllTexts
void removeAllTexts()
Definition: CloudViewer.cpp:2405
rtabmap::MainWindow::_occupancyGrid
rtabmap::OccupancyGrid * _occupancyGrid
Definition: MainWindow.h:394
rtabmap::PreferencesDialog::createLidar
Lidar * createLidar()
Definition: PreferencesDialog.cpp:6986
rtabmap::PreferencesDialog::getScanMaxRange
double getScanMaxRange(int index) const
Definition: PreferencesDialog.cpp:6041
rtabmap::ProgressDialog::setValue
void setValue(int value)
Definition: ProgressDialog.cpp:112
rtabmap::RegistrationInfo::inliersMeanDistance
float inliersMeanDistance
Definition: RegistrationInfo.h:82
CameraRGBD.h
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::MainWindow::selectRealSense
void selectRealSense()
Definition: MainWindow.cpp:7174
rtabmap::PreferencesDialog::kSrcRealSense2
@ kSrcRealSense2
Definition: PreferencesDialog.h:98
rtabmap::PreferencesDialog::init
void init(const QString &iniFilePath="")
Definition: PreferencesDialog.cpp:1678
rtabmap::CloudViewer::updateCloudPose
bool updateCloudPose(const std::string &id, const Transform &pose)
Definition: CloudViewer.cpp:547
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
ProgressDialog.h
rtabmap::CameraStereoTara::available
static bool available()
Definition: CameraStereoTara.cpp:45
type
rtabmap::PreferencesDialog::getOdomRegistrationApproach
int getOdomRegistrationApproach() const
Definition: PreferencesDialog.cpp:5823
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:534
rtabmap::OctoMap::writeBinary
bool writeBinary(const std::string &path)
Definition: OctoMap.cpp:1045
rtabmap::MainWindow::kClosing
@ kClosing
Definition: MainWindow.h:92
rtabmap::MainWindow::_createdScans
std::map< int, LaserScan > _createdScans
Definition: MainWindow.h:392
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:179
rtabmap::PreferencesDialog::kSrcStereoTara
@ kSrcStereoTara
Definition: PreferencesDialog.h:109
CameraStereo.h
rtabmap::PreferencesDialog::getBilateralSigmaR
double getBilateralSigmaR() const
Definition: PreferencesDialog.cpp:6343
rtabmap::RtabmapEvent
Definition: RtabmapEvent.h:42
rtabmap::MainWindow::kMonitoringPaused
@ kMonitoringPaused
Definition: MainWindow.h:97
rtabmap::MainWindow::moveEvent
virtual void moveEvent(QMoveEvent *anEvent)
Definition: MainWindow.cpp:5223
DBReader.h
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_CORE_EXPORT 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::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::MultiSessionLocWidget
Definition: MultiSessionLocWidget.h:45
rtabmap::PreferencesDialog::isPriorIgnored
bool isPriorIgnored() const
Definition: PreferencesDialog.cpp:7109
timer::restart
void restart()
rtabmap::MainWindow::_aboutDialog
AboutDialog * _aboutDialog
Definition: MainWindow.h:350
y
Matrix3f y
rtabmap::PreferencesDialog::getGeneralLoggerType
int getGeneralLoggerType() const
Definition: PreferencesDialog.cpp:5739
rtabmap::RtabmapEventCmd::kCmdDumpMemory
@ kCmdDumpMemory
Definition: RtabmapEvent.h:67
buffer
rtabmap::MainWindow::exportPosesRGBDSLAM
void exportPosesRGBDSLAM()
Definition: MainWindow.cpp:6236
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
Definition: global_map/OctoMap.h:179
rtabmap::MainWindow::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: MainWindow.cpp:781
rtabmap::PreferencesDialog::isCloudMeshing
bool isCloudMeshing() const
Definition: PreferencesDialog.cpp:5962
rtabmap::MainWindow::_defaultOpenDatabasePath
QString _defaultOpenDatabasePath
Definition: MainWindow.h:367
rtabmap::Odometry::kTypeMSCKF
@ kTypeMSCKF
Definition: Odometry.h:55
rtabmap::MainWindow::_cloudViewer
CloudViewer * _cloudViewer
Definition: MainWindow.h:416
rtabmap::CloudViewer::getBackgroundColor
const QColor & getBackgroundColor() const
Definition: CloudViewer.cpp:3170
rtabmap::PreferencesDialog::isIMUAccShown
bool isIMUAccShown() const
Definition: PreferencesDialog.cpp:5950
rtabmap::MainWindow::saveConfigGUI
virtual void saveConfigGUI()
Definition: MainWindow.cpp:5410
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
rtabmap::RtabmapEventInit::kError
@ kError
Definition: RtabmapEvent.h:145
true
#define true
Definition: ConvertUTF.c:57
rtabmap::MainWindow::updateCacheFromDatabase
void updateCacheFromDatabase()
Definition: MainWindow.cpp:7353
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
rtabmap::MainWindow::state
const State & state() const
Definition: MainWindow.h:294
rtabmap::MainWindow::rtabmapGoalStatusEventReceived
void rtabmapGoalStatusEventReceived(int status)
rtabmap::MainWindow::createAndAddScanToMap
void createAndAddScanToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:3982
rtabmap::OdometryInfo::localKeyFrames
int localKeyFrames
Definition: OdometryInfo.h:103
iterator
Lidar.h
rtabmap::PreferencesDialog::updateParameters
void updateParameters(const ParametersMap &parameters, bool setOtherParametersToDefault=false)
Definition: PreferencesDialog.cpp:4159
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
rtabmap::RtabmapEvent3DMap::getSignatures
const std::map< int, Signature > & getSignatures() const
Definition: RtabmapEvent.h:189
rtabmap::PreferencesDialog::isMarkerDetection
bool isMarkerDetection() const
Definition: PreferencesDialog.cpp:5954
rtabmap::MainWindow::exportPosesRGBDSLAMID
void exportPosesRGBDSLAMID()
Definition: MainWindow.cpp:6240
ULogger::Type
Type
Definition: ULogger.h:244
rtabmap::RtabmapGlobalPathEvent::getPlanningTime
double getPlanningTime() const
Definition: RtabmapEvent.h:227
rtabmap::MainWindow::setDefaultViews
virtual void setDefaultViews()
Definition: MainWindow.cpp:7680
rtabmap::PreferencesDialog::getBilateralSigmaS
double getBilateralSigmaS() const
Definition: PreferencesDialog.cpp:6339
rtabmap::PostProcessingDialog::sbaType
Optimizer::Type sbaType() const
Definition: PostProcessingDialog.cpp:280
rtabmap::PreferencesDialog::getScanPointSize
int getScanPointSize(int index) const
Definition: PreferencesDialog.cpp:6066
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:658
rtabmap::PreferencesDialog::getSubtractFilteringMinPts
int getSubtractFilteringMinPts() const
Definition: PreferencesDialog.cpp:6104
rtabmap::Registration::computeTransformation
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:167
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
util3d.h
rtabmap::PreferencesDialog::kSrcOpenNI2
@ kSrcOpenNI2
Definition: PreferencesDialog.h:93
rtabmap::PreferencesDialog::kSrcVideo
@ kSrcVideo
Definition: PreferencesDialog.h:118
rtabmap::PreferencesDialog::getMarkerLength
double getMarkerLength() const
Definition: PreferencesDialog.cpp:5958
rtabmap::LaserScan
Definition: LaserScan.h:37
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:825
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
rtabmap::PreferencesDialog::getVoxel
double getVoxel() const
Definition: PreferencesDialog.cpp:5875
rtabmap::LoopClosureViewer
Definition: LoopClosureViewer.h:43
rtabmap::MainWindow::_elapsedTime
QElapsedTimer * _elapsedTime
Definition: MainWindow.h:405
rtabmap::PreferencesDialog::getCloudFilteringRadius
double getCloudFilteringRadius() const
Definition: PreferencesDialog.cpp:6096
rtabmap::Odometry::kTypeOpenVINS
@ kTypeOpenVINS
Definition: Odometry.h:57
rtabmap::MainWindow::_progressDialog
ProgressDialog * _progressDialog
Definition: MainWindow.h:414
rtabmap::MainWindow::selectStereoDC1394
void selectStereoDC1394()
Definition: MainWindow.cpp:7193
rtabmap::PreferencesDialog::getSourceHistogramMethod
int getSourceHistogramMethod() const
Definition: PreferencesDialog.cpp:6351
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT 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::PreferencesDialog::getOdomQualityWarnThr
int getOdomQualityWarnThr() const
Definition: PreferencesDialog.cpp:5791
rtabmap::PreferencesDialog::getIMULocalTransform
Transform getIMULocalTransform() const
Definition: PreferencesDialog.cpp:6297
rtabmap::MainWindow::changeImgRateSetting
void changeImgRateSetting()
Definition: MainWindow.cpp:5310
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::MainWindow::_openedDatabasePath
QString _openedDatabasePath
Definition: MainWindow.h:366
rtabmap::MainWindow::_cachedLocalMaps
rtabmap::LocalGridCache _cachedLocalMaps
Definition: MainWindow.h:390
rtabmap::MainWindow::_likelihoodCurve
PdfPlotCurve * _likelihoodCurve
Definition: MainWindow.h:409
rtabmap::RtabmapEventCmd::kCmdRepublishData
@ kCmdRepublishData
Definition: RtabmapEvent.h:73
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::MainWindow::selectRealSense2Stereo
void selectRealSense2Stereo()
Definition: MainWindow.cpp:7188
rtabmap::MainWindow::exportImages
void exportImages()
Definition: MainWindow.cpp:8054
rtabmap::PreferencesDialog::getOctomapRenderingType
int getOctomapRenderingType() const
Definition: PreferencesDialog.cpp:5855
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:1113
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::GridMap
Definition: GridMap.h:42
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:923
rtabmap::MainWindow::setAspectRatio480p
void setAspectRatio480p()
Definition: MainWindow.cpp:7842
rtabmap::CloudViewer::getCloudVisibility
bool getCloudVisibility(const std::string &id)
Definition: CloudViewer.cpp:3214
rtabmap::CloudViewer::getDefaultBackgroundColor
const QColor & getDefaultBackgroundColor() const
Definition: CloudViewer.cpp:3156
rtabmap::CameraStereoZedOC::available
static bool available()
Definition: CameraStereoZedOC.cpp:499
rtabmap::CameraFreenect2::available
static bool available()
Definition: CameraFreenect2.cpp:45
KeypointItem.h
rtabmap::PreferencesDialog::isImagesKept
bool isImagesKept() const
Definition: PreferencesDialog.cpp:7077
rtabmap::RtabmapEventCmd::kCmdResetMemory
@ kCmdResetMemory
Definition: RtabmapEvent.h:64
rtabmap::MainWindow::_recovering
bool _recovering
Definition: MainWindow.h:362
rtabmap::Statistics::proximityDetectionId
int proximityDetectionId() const
Definition: Statistics.h:268
rtabmap::MainWindow::selectFreenect2
void selectFreenect2()
Definition: MainWindow.cpp:7159
rtabmap::PreferencesDialog::getOdomBufferSize
int getOdomBufferSize() const
Definition: PreferencesDialog.cpp:7067
rtabmap::OccupancyGrid
Definition: global_map/OccupancyGrid.h:40
rtabmap::MainWindow::selectOpenniCv
void selectOpenniCv()
Definition: MainWindow.cpp:7144
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::PreferencesDialog::getLidarSourceDriver
PreferencesDialog::Src getLidarSourceDriver() const
Definition: PreferencesDialog.cpp:6264
rtabmap::MainWindow::_cachedMemoryUsage
long _cachedMemoryUsage
Definition: MainWindow.h:378
rtabmap::OdometryInfo::transform
Transform transform
Definition: OdometryInfo.h:115
rtabmap::MainWindow::exportPosesKITTI
void exportPosesKITTI()
Definition: MainWindow.cpp:6244
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::SensorCaptureThread::setDistortionModel
void setDistortionModel(const std::string &path)
Definition: SensorCaptureThread.cpp:196
indices
indices
rtabmap::PreferencesDialog::getCloudRoiRatios
std::vector< float > getCloudRoiRatios(int index) const
Definition: PreferencesDialog.cpp:5997
rtabmap::ProgressDialog::setAutoClose
void setAutoClose(bool on, int delayedClosingTimeSec=-1)
Definition: ProgressDialog.cpp:88
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
rtabmap::MainWindow::rtabmapEvent3DMapReceived
void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &event)
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_CORE_EXPORT 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::PreferencesDialog::getNormalKSearch
int getNormalKSearch() const
Definition: PreferencesDialog.cpp:5895
rtabmap::MainWindow::odometryProcessed
void odometryProcessed()
ULogger::setTreadIdFilter
static void setTreadIdFilter(const std::set< unsigned long > &ids)
Definition: ULogger.h:354
rtabmap::PreferencesDialog::getTmpIniFilePath
virtual QString getTmpIniFilePath() const
Definition: PreferencesDialog.cpp:2408
fabs
Real fabs(const Real &a)
rtabmap::MainWindow::_depthCalibrationDialog
DepthCalibrationDialog * _depthCalibrationDialog
Definition: MainWindow.h:354
rtabmap::RtabmapEventCmd::kCmdInit
@ kCmdInit
Definition: RtabmapEvent.h:63
rtabmap::Odometry::kTypeViso2
@ kTypeViso2
Definition: Odometry.h:50
Parameters.h
rtabmap::PreferencesDialog::kSrcStereoDepthAI
@ kSrcStereoDepthAI
Definition: PreferencesDialog.h:113
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:670
uKeysList
std::list< K > uKeysList(const std::multimap< K, V > &mm)
Definition: UStl.h:84
UEvent
Definition: UEvent.h:57
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::MainWindow::_cachedSignatures
QMap< int, Signature > _cachedSignatures
Definition: MainWindow.h:377
rtabmap::MainWindow::editDatabase
void editDatabase()
Definition: MainWindow.cpp:5682
n
int n
GainCompensator.h
Odometry.h
rtabmap::OdometryInfo::localBundleModels
std::map< int, std::vector< CameraModel > > localBundleModels
Definition: OdometryInfo.h:108
rtabmap::MainWindow::selectStereoTara
void selectStereoTara()
Definition: MainWindow.cpp:7211
labels
std::vector< std::string > labels
rtabmap::MainWindow::exportPosesTORO
void exportPosesTORO()
Definition: MainWindow.cpp:6248
rtabmap::OdometryInfo::type
int type
Definition: OdometryInfo.h:125
rtabmap::PreferencesDialog::kSrcImages
@ kSrcImages
Definition: PreferencesDialog.h:117
rtabmap::CloudViewer::removeText
void removeText(const std::string &id)
Definition: CloudViewer.cpp:2390
rtabmap::MainWindow::startDetection
virtual void startDetection()
Definition: MainWindow.cpp:5725
rtabmap::MainWindow::kStartingDetection
@ kStartingDetection
Definition: MainWindow.h:93
rtabmap::MainWindow::pauseDetection
virtual void pauseDetection()
Definition: MainWindow.cpp:6074
rtabmap::PostProcessingDialog::sbaVariance
double sbaVariance() const
Definition: PostProcessingDialog.cpp:276
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::PreferencesDialog::kSrcStereoZedOC
@ kSrcStereoZedOC
Definition: PreferencesDialog.h:112
rtabmap::PreferencesDialog::kSrcStereoUsb
@ kSrcStereoUsb
Definition: PreferencesDialog.h:108
rtabmap::SensorCapture::odomProvided
virtual bool odomProvided() const
Definition: SensorCapture.h:57
rtabmap::MainWindow::statsProcessed
void statsProcessed()
rtabmap::Camera::isCalibrated
virtual bool isCalibrated() const =0
rtabmap::SensorData::setRGBDImage
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:227
AboutDialog.h
rtabmap::Statistics::odomCacheConstraints
const std::multimap< int, Link > & odomCacheConstraints() const
Definition: Statistics.h:290
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::DataRecorder
Definition: DataRecorder.h:46
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::OdometryInfo::localBundlePoses
std::map< int, Transform > localBundlePoses
Definition: OdometryInfo.h:107
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
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:1143
rtabmap::SensorCaptureInfo::odomPose
Transform odomPose
Definition: SensorCaptureInfo.h:74
rtabmap::GlobalMap::addedNodes
const std::map< int, Transform > & addedNodes() const
Definition: GlobalMap.h:62
rtabmap::CloudViewer::getAddedLines
const std::set< std::string > & getAddedLines() const
Definition: CloudViewer.h:237
rtabmap::MainWindow::setLoopClosureViewer
void setLoopClosureViewer(rtabmap::LoopClosureViewer *loopClosureViewer)
Definition: MainWindow.cpp:771
rtabmap::SensorCaptureThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: SensorCaptureThread.cpp:224
rtabmap::PreferencesDialog::saveSettings
void saveSettings()
Definition: PreferencesDialog.cpp:1709
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:896
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
LOG_FILE_NAME
#define LOG_FILE_NAME
Definition: MainWindow.cpp:130
rtabmap::MainWindow::_exportPosesFileName
QMap< int, QString > _exportPosesFileName
Definition: MainWindow.h:421
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
Definition: Statistics.h:292
rtabmap::DepthCalibrationDialog
Definition: DepthCalibrationDialog.h:49
PdfPlot.h
UFATAL
#define UFATAL(...)
rtabmap::PreferencesDialog::getCloudMaxDepth
double getCloudMaxDepth(int index) const
Definition: PreferencesDialog.cpp:5987
rtabmap::PreferencesDialog::isBilateralFiltering
bool isBilateralFiltering() const
Definition: PreferencesDialog.cpp:6335
rtabmap::PreferencesDialog::isSourceStereoDepthGenerated
bool isSourceStereoDepthGenerated() const
Definition: PreferencesDialog.cpp:6359
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::PreferencesDialog::getCloudOpacity
double getCloudOpacity(int index) const
Definition: PreferencesDialog.cpp:6020
rtabmap::PreferencesDialog::isScansShown
bool isScansShown(int index) const
Definition: PreferencesDialog.cpp:6031
rtabmap::MultiSessionLocWidget::clear
void clear()
Definition: MultiSessionLocWidget.cpp:194
rtabmap::ExportBundlerDialog
Definition: ExportBundlerDialog.h:42
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT 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:1268
rtabmap::Statistics::posterior
const std::map< int, float > & posterior() const
Definition: Statistics.h:282
rtabmap::ProgressDialog::isCanceled
bool isCanceled() const
Definition: ProgressDialog.h:57
rtabmap::PreferencesDialog::imageRejectedShown
bool imageRejectedShown() const
Definition: PreferencesDialog.cpp:5767
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
rtabmap::MainWindow::setAspectRatio720p
void setAspectRatio720p()
Definition: MainWindow.cpp:7847
ULogger::setEventLevel
static void setEventLevel(ULogger::Level eventSentLevel)
Definition: ULogger.h:348
RecoveryState.h
rtabmap::PreferencesDialog::getFeaturesPointSize
int getFeaturesPointSize(int index) const
Definition: PreferencesDialog.cpp:6082
ULogger::setPrintTime
static void setPrintTime(bool printTime)
Definition: ULogger.h:273
rtabmap::MainWindow::drawLandmarks
void drawLandmarks(cv::Mat &image, const Signature &signature)
Definition: MainWindow.cpp:5143
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::CloudViewer::addElevationMap
bool addElevationMap(const cv::Mat &map32FC1, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1610
data
int data[]
rtabmap::PreferencesDialog::landmarkVisSize
double landmarkVisSize() const
Definition: PreferencesDialog.cpp:5936
rtabmap::OdometryInfo::cornerInliers
std::vector< int > cornerInliers
Definition: OdometryInfo.h:135
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:2146
delta
def delta(g0, g1)
util3d_transforms.h
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::PdfPlotCurve::setData
void setData(const QMap< int, float > &dataMap, const QMap< int, int > &weightsMap)
Definition: PdfPlot.cpp:136
rtabmap::MainWindow::_autoScreenCaptureOdomSync
bool _autoScreenCaptureOdomSync
Definition: MainWindow.h:422
UPlotCurveThreshold
Definition: UPlot.h:263
rtabmap::RtabmapEvent3DMap
Definition: RtabmapEvent.h:172
rtabmap::MainWindow::rtabmapEvent3DMapProcessed
void rtabmapEvent3DMapProcessed()
rtabmap::MainWindow::statsReceived
void statsReceived(const rtabmap::Statistics &)
rtabmap::MainWindow::rtabmapLabelErrorReceived
void rtabmapLabelErrorReceived(int id, const QString &label)
rtabmap::MainWindow::processRtabmapEvent3DMap
void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &event)
Definition: MainWindow.cpp:4666
rtabmap::Optimizer::Type
Type
Definition: Optimizer.h:64
rtabmap::MainWindow::showEvent
virtual void showEvent(QShowEvent *anEvent)
Definition: MainWindow.cpp:5217
j
std::ptrdiff_t j
rtabmap::PostProcessingDialog::clusterAngle
double clusterAngle() const
Definition: PostProcessingDialog.cpp:237
rtabmap::PreferencesDialog::getSourceScanDownsampleStep
int getSourceScanDownsampleStep() const
Definition: PreferencesDialog.cpp:6375
rtabmap::MultiSessionLocWidget::updateView
void updateView(const Signature &lastSignature, const Statistics &stats)
Definition: MultiSessionLocWidget.cpp:72
rtabmap::PreferencesDialog::isMissingCacheRepublished
bool isMissingCacheRepublished() const
Definition: PreferencesDialog.cpp:7081
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::graph::computePathLength
float RTABMAP_CORE_EXPORT computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2346
stats
bool stats
rtabmap::MainWindow::createAndAddFeaturesToMap
void createAndAddFeaturesToMap(int nodeId, const Transform &pose, int mapId)
Definition: MainWindow.cpp:4293
rtabmap::CloudViewer::setBackfaceCulling
void setBackfaceCulling(bool enabled, bool frontfaceCulling)
Definition: CloudViewer.cpp:2732
rtabmap::PreferencesDialog::isCloudsShown
bool isCloudsShown(int index) const
Definition: PreferencesDialog.cpp:5836
rtabmap::MainWindow::stopDetection
virtual void stopDetection()
Definition: MainWindow.cpp:6108
rtabmap::PreferencesDialog::getScanFloorFilteringHeight
double getScanFloorFilteringHeight() const
Definition: PreferencesDialog.cpp:5907
rtabmap::CameraDepthAI::available
static bool available()
Definition: CameraDepthAI.cpp:39
rtabmap::CameraSeerSense::available
static bool available()
Definition: CameraSeerSense.cpp:6
UConversion.h
Some conversion functions.
rtabmap::MainWindow::triggerNewMap
virtual void triggerNewMap()
Definition: MainWindow.cpp:8332
rtabmap::LocalGridCache::clear
void clear(bool temporaryOnly=false)
Definition: LocalGrid.cpp:103
rtabmap::MainWindow::sendWaypoints
void sendWaypoints()
Definition: MainWindow.cpp:7275
rtabmap::PreferencesDialog::getScanCeilingFilteringHeight
double getScanCeilingFilteringHeight() const
Definition: PreferencesDialog.cpp:5903
rtabmap::PreferencesDialog::isOdomDisabled
bool isOdomDisabled() const
Definition: PreferencesDialog.cpp:5815
rtabmap::PreferencesDialog::getDetectionRate
float getDetectionRate() const
Definition: PreferencesDialog.cpp:7093
rtabmap::MainWindow::openDatabase
virtual void openDatabase()
Definition: MainWindow.cpp:5521
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::PreferencesDialog::getIniFilePath
virtual QString getIniFilePath() const
Definition: PreferencesDialog.cpp:2398
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:2476
CloudViewer.h
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
rtabmap::Signature::getLandmarks
const std::map< int, Link > & getLandmarks() const
Definition: Signature.h:94
UEventsManager::createPipe
static void createPipe(const UEventsSender *sender, const UEventsHandler *receiver, const std::string &eventName)
Definition: UEventsManager.cpp:67
rtabmap::PreferencesDialog::getSubtractFilteringRadius
double getSubtractFilteringRadius() const
Definition: PreferencesDialog.cpp:6108
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::PreferencesDialog::getGeneralInputRate
double getGeneralInputRate() const
Definition: PreferencesDialog.cpp:6163
UEventsManager::addHandler
static void addHandler(UEventsHandler *handler)
Definition: UEventsManager.cpp:28
util3d_filtering.h
rtabmap::GlobalMap::update
bool update(const std::map< int, Transform > &poses)
Definition: GlobalMap.cpp:102
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
rtabmap::MainWindow::_cachedWordsCount
std::map< int, float > _cachedWordsCount
Definition: MainWindow.h:388
rtabmap::PreferencesDialog::getWorkingDirectory
QString getWorkingDirectory() const
Definition: PreferencesDialog.cpp:2393
rtabmap::CloudViewer::updateCameraFrustum
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
Definition: CloudViewer.cpp:3095
rtabmap::MainWindow::generateGraphDOT
void generateGraphDOT()
Definition: MainWindow.cpp:6196
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
rtabmap::MainWindow::_octomap
rtabmap::OctoMap * _octomap
Definition: MainWindow.h:395
rtabmap::MainWindow::openWorkingDirectory
void openWorkingDirectory()
Definition: MainWindow.cpp:7095
Signature.h
rtabmap::MainWindow::selectRealSense2L515
void selectRealSense2L515()
Definition: MainWindow.cpp:7183
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1777
rtabmap::PreferencesDialog::getCloudVoxelSizeScan
double getCloudVoxelSizeScan(int index) const
Definition: PreferencesDialog.cpp:6051
rtabmap::OdometryInfo::localBundleConstraints
int localBundleConstraints
Definition: OdometryInfo.h:105
rtabmap::CloudViewer::removeAllGraphs
void removeAllGraphs()
Definition: CloudViewer.cpp:2349
rtabmap::MainWindow::_firstStamp
double _firstStamp
Definition: MainWindow.h:359
rtabmap::Odometry::kTypeFovis
@ kTypeFovis
Definition: Odometry.h:49
rtabmap::MainWindow::kIdle
@ kIdle
Definition: MainWindow.h:88
GridMap.h
rtabmap::PreferencesDialog::kPanelAll
@ kPanelAll
Definition: PreferencesDialog.h:80
rtabmap::OdometryInfo::localScanMapSize
int localScanMapSize
Definition: OdometryInfo.h:102
rtabmap::PreferencesDialog::isSourceDatabaseStampsUsed
bool isSourceDatabaseStampsUsed() const
Definition: PreferencesDialog.cpp:6311
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
first
constexpr int first(int i)
rtabmap::PreferencesDialog::getNormalRadiusSearch
double getNormalRadiusSearch() const
Definition: PreferencesDialog.cpp:5899
arg
rtabmap::Statistics::labels
const std::map< int, std::string > & labels() const
Definition: Statistics.h:280
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:120
rtabmap::PreferencesDialog::kSrcK4A
@ kSrcK4A
Definition: PreferencesDialog.h:99
rtabmap::MainWindow::_imuThread
rtabmap::IMUThread * _imuThread
Definition: MainWindow.h:346
rtabmap::PreferencesDialog::isCloudMeshingQuad
bool isCloudMeshingQuad() const
Definition: PreferencesDialog.cpp:5970
rtabmap::MainWindow::selectStereoZedOC
void selectStereoZedOC()
Definition: MainWindow.cpp:7206
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3327
rtabmap::SensorEvent::kCodeNoMoreImages
@ kCodeNoMoreImages
Definition: SensorEvent.h:43
rtabmap::PreferencesDialog::isLabelsShown
bool isLabelsShown() const
Definition: PreferencesDialog.cpp:5924
rtabmap::RtabmapLabelErrorEvent::label
const std::string & label() const
Definition: RtabmapEvent.h:246
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UCv2Qt.h
rtabmap::MainWindow::progressDialog
rtabmap::ProgressDialog * progressDialog()
Definition: MainWindow.h:310
rtabmap::MainWindow::_autoScreenCaptureCachedImages
QMap< QString, QByteArray > _autoScreenCaptureCachedImages
Definition: MainWindow.h:425
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:132
rtabmap::PreferencesDialog::getCloudMeshingTriangleSize
int getCloudMeshingTriangleSize()
Definition: PreferencesDialog.cpp:5978
rtabmap::MainWindow::applyPrefSettings
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
Definition: MainWindow.cpp:4866
rtabmap::OccupancyGrid::clear
virtual void clear()
Definition: OccupancyGrid.cpp:91
rtabmap::Statistics::defaultData
static const std::map< std::string, float > & defaultData()
Definition: Statistics.cpp:36
rtabmap::GridMap::createTerrainMesh
pcl::PolygonMesh::Ptr createTerrainMesh() const
Definition: GridMap.cpp:161
rtabmap::MainWindow::newDatabase
virtual void newDatabase()
Definition: MainWindow.cpp:5431
ULogger::kInfo
@ kInfo
Definition: ULogger.h:252
rtabmap::SensorCaptureThread::odomProvided
bool odomProvided() const
Definition: SensorCaptureThread.cpp:298
rtabmap::PreferencesDialog::setCurrentPanelToSource
void setCurrentPanelToSource()
Definition: PreferencesDialog.cpp:1695
UThread::kill
void kill()
Definition: UThread.cpp:48
rtabmap::MainWindow::_graphSavingFileName
QString _graphSavingFileName
Definition: MainWindow.h:419
rtabmap::SensorCaptureThread::setStereoExposureCompensation
void setStereoExposureCompensation(bool enabled)
Definition: SensorCaptureThread.h:124
SensorEvent.h
rtabmap::SensorCaptureThread::enableFeatureDetection
void enableFeatureDetection(const ParametersMap &parameters=ParametersMap())
Definition: SensorCaptureThread.cpp:237
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
Definition: RegistrationInfo.h:75
rtabmap::StatItem
Definition: StatsToolBox.h:44
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1992
rtabmap::AboutDialog
Definition: AboutDialog.h:40
UCvMat2QImageThread::getQImage
QImage & getQImage()
Definition: UCv2Qt.h:269
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:2299
info
else if n * info
rtabmap::RegistrationInfo::icpInliersRatio
float icpInliersRatio
Definition: RegistrationInfo.h:90
rtabmap::RegistrationInfo::inliers
int inliers
Definition: RegistrationInfo.h:80
rtabmap::MainWindow::removeLabel
void removeLabel()
Definition: MainWindow.cpp:7342
rtabmap::MainWindow::setAspectRatio360p
void setAspectRatio360p()
Definition: MainWindow.cpp:7837
rtabmap::PreferencesDialog::isFeaturesShown
bool isFeaturesShown(int index) const
Definition: PreferencesDialog.cpp:6072
rtabmap::MainWindow::cancelProgress
void cancelProgress()
Definition: MainWindow.cpp:5376
rtabmap::MainWindow::_elevationMap
rtabmap::GridMap * _elevationMap
Definition: MainWindow.h:396
rtabmap::PreferencesDialog::kSrcStereoVideo
@ kSrcStereoVideo
Definition: PreferencesDialog.h:106
rtabmap::Statistics::currentGoalId
int currentGoalId() const
Definition: Statistics.h:286
rtabmap::MainWindow::selectScreenCaptureFormat
void selectScreenCaptureFormat(bool checked)
Definition: MainWindow.cpp:7703
rtabmap::OdometryInfo::localScanMap
LaserScan localScanMap
Definition: OdometryInfo.h:130
rtabmap::MainWindow::processStats
virtual void processStats(const rtabmap::Statistics &stat)
Definition: MainWindow.cpp:1943
rtabmap::CloudViewer::setCameraTargetFollow
void setCameraTargetFollow(bool enabled=true)
Definition: CloudViewer.cpp:3293
rtabmap::PreferencesDialog::isSourceScanFromDepth
bool isSourceScanFromDepth() const
Definition: PreferencesDialog.cpp:6367
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:1810
rtabmap::MainWindow::_dataRecorder
DataRecorder * _dataRecorder
Definition: MainWindow.h:355
rtabmap::util3d::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
Definition: util3d_filtering.cpp:1423
rtabmap::RtabmapEventInit::kClosed
@ kClosed
Definition: RtabmapEvent.h:143
rtabmap::SensorCaptureThread::setMirroringEnabled
void setMirroringEnabled(bool enabled)
Definition: SensorCaptureThread.h:123
rtabmap::MainWindow::loadFigures
void loadFigures()
Definition: MainWindow.cpp:7612
UASSERT
#define UASSERT(condition)
ULogEvent
Definition: ULogger.h:121
rtabmap::MainWindow::dumpThePrediction
void dumpThePrediction()
Definition: MainWindow.cpp:7256
DepthCalibrationDialog.h
rtabmap::RtabmapLabelErrorEvent::id
int id() const
Definition: RtabmapEvent.h:245
rtabmap::MainWindow::cameraInfoReceived
void cameraInfoReceived(const rtabmap::SensorCaptureInfo &)
rtabmap::RtabmapEventCmd::kCmdGoal
@ kCmdGoal
Definition: RtabmapEvent.h:77
rtabmap::MainWindow::_rectCameraModelsOdom
std::vector< CameraModel > _rectCameraModelsOdom
Definition: MainWindow.h:375
z
z
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
x
x
rtabmap::SensorData::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: SensorData.cpp:810
UThread::start
void start()
Definition: UThread.cpp:122
rtabmap::PostProcessingDialog::iterations
int iterations() const
Definition: PostProcessingDialog.cpp:242
iterator::value
object value
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:3241
rtabmap::PreferencesDialog::kPanelSource
@ kPanelSource
Definition: PreferencesDialog.h:78
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::IMUThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: IMUThread.cpp:88
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:128
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:3115
rtabmap::OdometryEvent::info
const OdometryInfo & info() const
Definition: OdometryEvent.h:89
rtabmap::util3d::filterNotUsedVerticesFromMesh
std::vector< int > RTABMAP_CORE_EXPORT 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::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:99
rtabmap::MainWindow::_currentGTPosesMap
std::map< int, Transform > _currentGTPosesMap
Definition: MainWindow.h:380
rtabmap::PreferencesDialog::kPanelCloudRendering
@ kPanelCloudRendering
Definition: PreferencesDialog.h:76
rtabmap::PreferencesDialog::isStatisticsPublished
bool isStatisticsPublished() const
Definition: PreferencesDialog.cpp:7047
rtabmap::OdometryInfo::copyWithoutData
OdometryInfo copyWithoutData() const
Definition: OdometryInfo.h:65
DBDriver.h
rtabmap::MainWindow::setAspectRatio240p
void setAspectRatio240p()
Definition: MainWindow.cpp:7832
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::MainWindow::_currentPosesMap
std::map< int, Transform > _currentPosesMap
Definition: MainWindow.h:379
rtabmap::PreferencesDialog::kSrcOpenNI_PCL
@ kSrcOpenNI_PCL
Definition: PreferencesDialog.h:89
rtabmap::PreferencesDialog::getIMUFilteringStrategy
int getIMUFilteringStrategy() const
Definition: PreferencesDialog.cpp:6319
rtabmap::CloudViewer::setCloudVisibility
void setCloudVisibility(const std::string &id, bool isVisible)
Definition: CloudViewer.cpp:3181
rtabmap::PreferencesDialog::getGeneralLoggerLevel
int getGeneralLoggerLevel() const
Definition: PreferencesDialog.cpp:5727
rtabmap::PreferencesDialog::getGeneralLoggerThreads
std::vector< std::string > getGeneralLoggerThreads() const
Definition: PreferencesDialog.cpp:5751
rtabmap::RtabmapGlobalPathEvent::getGoalLabel
const std::string & getGoalLabel() const
Definition: RtabmapEvent.h:226
rtabmap::RegistrationInfo::icpTranslation
float icpTranslation
Definition: RegistrationInfo.h:91
rtabmap::MainWindow::_currentLabels
std::map< int, std::string > _currentLabels
Definition: MainWindow.h:383
rtabmap::MainWindow::_exportBundlerDialog
ExportBundlerDialog * _exportBundlerDialog
Definition: MainWindow.h:352
rtabmap::MainWindow::exportClouds
void exportClouds()
Definition: MainWindow.cpp:7941
rtabmap::MainWindow::_createdCloudsMemoryUsage
long _createdCloudsMemoryUsage
Definition: MainWindow.h:385
rtabmap::MainWindow::configGUIModified
void configGUIModified()
Definition: MainWindow.cpp:5382
rtabmap::PreferencesDialog::isPosteriorGraphView
bool isPosteriorGraphView() const
Definition: PreferencesDialog.cpp:5799
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::Statistics::extended
bool extended() const
Definition: Statistics.h:263
CameraRGB.h
util3d_mapping.h
OdometryEvent.h
rtabmap::CameraStereoZed::available
static bool available()
Definition: CameraStereoZed.cpp:235
rtabmap::Camera
Definition: Camera.h:43
rtabmap::PreferencesDialog::isOctomapShown
bool isOctomapShown() const
Definition: PreferencesDialog.cpp:5848
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
time
#define time
uKeysSet
std::set< K > uKeysSet(const std::map< K, V > &m)
Definition: UStl.h:186
rtabmap::SensorCaptureThread::setStereoToDepth
void setStereoToDepth(bool enabled)
Definition: SensorCaptureThread.h:128
rtabmap::PreferencesDialog::isCloudsKept
bool isCloudsKept() const
Definition: PreferencesDialog.cpp:7085
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::MainWindow::_newDatabasePath
QString _newDatabasePath
Definition: MainWindow.h:364
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:1024
rtabmap::Statistics::likelihood
const std::map< int, float > & likelihood() const
Definition: Statistics.h:283
rtabmap::RtabmapEventCmd::kCmdLabel
@ kCmdLabel
Definition: RtabmapEvent.h:79
rtabmap::SensorCapture
Definition: SensorCapture.h:49
rtabmap::PreferencesDialog
Definition: PreferencesDialog.h:68
rtabmap::PreferencesDialog::kSrcDatabase
@ kSrcDatabase
Definition: PreferencesDialog.h:120
rtabmap::MainWindow::kPaused
@ kPaused
Definition: MainWindow.h:95
rtabmap::MainWindow::selectMyntEyeS
void selectMyntEyeS()
Definition: MainWindow.cpp:7221
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::PreferencesDialog::kSrcStereoImages
@ kSrcStereoImages
Definition: PreferencesDialog.h:105
rtabmap::PreferencesDialog::getSourceDriverStr
QString getSourceDriverStr() const
Definition: PreferencesDialog.cpp:6213
rtabmap::CloudViewer::removeFrustum
void removeFrustum(const std::string &id)
Definition: CloudViewer.cpp:2268
rtabmap::PreferencesDialog::getGeneralLoggerPauseLevel
int getGeneralLoggerPauseLevel() const
Definition: PreferencesDialog.cpp:5735
rtabmap::PreferencesDialog::getScanNormalKSearch
int getScanNormalKSearch() const
Definition: PreferencesDialog.cpp:5911
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::graph::findNearestNodes
std::map< int, float > RTABMAP_CORE_EXPORT findNearestNodes(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2202
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:399
rtabmap::PreferencesDialog::isLandmarksShown
bool isLandmarksShown() const
Definition: PreferencesDialog.cpp:5932
rtabmap::PreferencesDialog::imageHighestHypShown
bool imageHighestHypShown() const
Definition: PreferencesDialog.cpp:5771
rtabmap::PreferencesDialog::getGridMapOpacity
double getGridMapOpacity() const
Definition: PreferencesDialog.cpp:6156
path
path
rtabmap::CloudViewer::resetCamera
void resetCamera()
Definition: CloudViewer.cpp:2530
rtabmap::MainWindow::~MainWindow
virtual ~MainWindow()
Definition: MainWindow.cpp:723
str
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CloudViewer::removeOctomap
void removeOctomap()
Definition: CloudViewer.cpp:1324
rtabmap::LocalGridCache::add
void add(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint=cv::Point3f(0, 0, 0))
Definition: LocalGrid.cpp:55
rtabmap::MainWindow::_sensorCapture
rtabmap::SensorCaptureThread * _sensorCapture
Definition: MainWindow.h:344
UWARN
#define UWARN(...)
rtabmap::MainWindow::changeDetectionRateSetting
void changeDetectionRateSetting()
Definition: MainWindow.cpp:5315
rtabmap::Statistics::rawLikelihood
const std::map< int, float > & rawLikelihood() const
Definition: Statistics.h:284
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::Odometry::kTypeVINS
@ kTypeVINS
Definition: Odometry.h:56
rtabmap::MainWindow::kDetecting
@ kDetecting
Definition: MainWindow.h:94
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
rtabmap::DBDriver
Definition: DBDriver.h:62
rtabmap::PreferencesDialog::getCloudFilteringAngle
double getCloudFilteringAngle() const
Definition: PreferencesDialog.cpp:6100
rtabmap::MainWindow::getCustomParameters
virtual ParametersMap getCustomParameters()
Definition: MainWindow.h:320
rtabmap::GridMap::createHeightMap
cv::Mat createHeightMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:57
rtabmap::MainWindow::setAspectRatio16_10
void setAspectRatio16_10()
Definition: MainWindow.cpp:7822
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::isSLAMMode
bool isSLAMMode() const
Definition: PreferencesDialog.cpp:7097
rtabmap::PreferencesDialog::isOdomSensorAsGt
bool isOdomSensorAsGt() const
Definition: PreferencesDialog.cpp:5819
rtabmap::MainWindow::selectStereoUsb
void selectStereoUsb()
Definition: MainWindow.cpp:7216
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::MainWindow::openPreferencesSource
virtual void openPreferencesSource()
Definition: MainWindow.cpp:7673
rtabmap::MainWindow::updateEditMenu
void updateEditMenu()
Definition: MainWindow.cpp:7118
nodes
KeyVector nodes
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::MainWindow::_refIds
QVector< int > _refIds
Definition: MainWindow.h:427
rtabmap::PreferencesDialog::getGridUIResolution
double getGridUIResolution() const
Definition: PreferencesDialog.cpp:6116
rtabmap::PostProcessingDialog::isDetectMoreLoopClosures
bool isDetectMoreLoopClosures() const
Definition: PostProcessingDialog.cpp:227
rtabmap::MainWindow::_posteriorCurve
PdfPlotCurve * _posteriorCurve
Definition: MainWindow.h:408
rtabmap::PreferencesDialog::loadWindowGeometry
void loadWindowGeometry(QWidget *window)
Definition: PreferencesDialog.cpp:3868
rtabmap::PreferencesDialog::getTimeLimit
float getTimeLimit() const
Definition: PreferencesDialog.cpp:7089
rtabmap::MainWindow::setAspectRatio4_3
void setAspectRatio4_3()
Definition: MainWindow.cpp:7827
rtabmap::Statistics::getSignaturesData
const std::map< int, Signature > & getSignaturesData() const
Definition: Statistics.h:273
rtabmap::PreferencesDialog::getCloudDecimation
int getCloudDecimation(int index) const
Definition: PreferencesDialog.cpp:5982
rtabmap::CloudViewer::getAddedClouds
const QMap< std::string, Transform > & getAddedClouds() const
Definition: CloudViewer.h:338
rtabmap::MainWindow::loopClosureThrChanged
void loopClosureThrChanged(qreal)
rtabmap::PreferencesDialog::isGroundTruthAligned
bool isGroundTruthAligned() const
Definition: PreferencesDialog.cpp:5831
rtabmap::MainWindow::dumpTheMemory
void dumpTheMemory()
Definition: MainWindow.cpp:7251
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
rtabmap::DatabaseViewer::isSavedMaximized
bool isSavedMaximized() const
Definition: DatabaseViewer.h:76
rtabmap::PreferencesDialog::getCeilingFilteringHeight
double getCeilingFilteringHeight() const
Definition: PreferencesDialog.cpp:5887
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::MultiSessionLocWidget::getImageView
ImageView * getImageView()
Definition: MultiSessionLocWidget.h:56
rtabmap::RegistrationInfo::matches
int matches
Definition: RegistrationInfo.h:85
UPlot.h
rtabmap::MainWindow::selectDepthAIOAKDLite
void selectDepthAIOAKDLite()
Definition: MainWindow.cpp:7231
MainWindow.h
rtabmap::PreferencesDialog::kSrcFreenect2
@ kSrcFreenect2
Definition: PreferencesDialog.h:94
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1530
rtabmap::PreferencesDialog::getSourceImageDecimation
int getSourceImageDecimation() const
Definition: PreferencesDialog.cpp:6347
rtabmap::CameraOpenNICV::available
static bool available()
Definition: CameraOpenNICV.cpp:36
Memory.h
empty
rtabmap::MainWindow::saveFigures
void saveFigures()
Definition: MainWindow.cpp:7591
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
util2d.h
rtabmap::PdfPlotCurve
Definition: PdfPlot.h:62
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
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_CORE_EXPORT 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::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
rtabmap::MainWindow::currentVisiblePosesMap
std::map< int, Transform > currentVisiblePosesMap() const
Definition: MainWindow.cpp:752
rtabmap::MainWindow::notifyNoMoreImages
void notifyNoMoreImages()
Definition: MainWindow.cpp:6170
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:86
rtabmap::MainWindow::selectFreenect
void selectFreenect()
Definition: MainWindow.cpp:7139
rtabmap::RtabmapEventInit::kClosing
@ kClosing
Definition: RtabmapEvent.h:142
rtabmap::CameraMyntEye::available
static bool available()
Definition: CameraMyntEye.cpp:506
rtabmap::RtabmapEventCmd::kCmdCancelGoal
@ kCmdCancelGoal
Definition: RtabmapEvent.h:78
rtabmap::MainWindow::selectXvisioSeerSense
void selectXvisioSeerSense()
Definition: MainWindow.cpp:7241
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::OdometryInfo::distanceTravelled
float distanceTravelled
Definition: OdometryInfo.h:120
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::PostProcessingDialog::isRefineLoopClosureLinks
bool isRefineLoopClosureLinks() const
Definition: PostProcessingDialog.cpp:262
rtabmap::PreferencesDialog::getElevationMapShown
int getElevationMapShown() const
Definition: PreferencesDialog.cpp:6124
Graph.h
rtabmap::SensorEvent::info
const SensorCaptureInfo & info() const
Definition: SensorEvent.h:81
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:261
rtabmap::PreferencesDialog::createOdomSensor
SensorCapture * createOdomSensor(Transform &extrinsics, double &timeOffset, float &scaleFactor, double &waitTime)
Definition: PreferencesDialog.cpp:6960
rtabmap::LoopClosureViewer::setData
void setData(const Signature &sA, const Signature &sB)
Definition: LoopClosureViewer.cpp:60
rtabmap::PreferencesDialog::getScanOpacity
double getScanOpacity(int index) const
Definition: PreferencesDialog.cpp:6061
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::RegistrationInfo::icpRotation
float icpRotation
Definition: RegistrationInfo.h:92
rtabmap::MainWindow::selectStereoFlyCapture2
void selectStereoFlyCapture2()
Definition: MainWindow.cpp:7198
rtabmap::PostProcessingDialog
Definition: PostProcessingDialog.h:43
rtabmap::MainWindow::anchorCloudsToGroundTruth
void anchorCloudsToGroundTruth()
Definition: MainWindow.cpp:7492
rtabmap::PreferencesDialog::getSourceScanNormalsK
int getSourceScanNormalsK() const
Definition: PreferencesDialog.cpp:6391
rtabmap::PreferencesDialog::getScanColorScheme
int getScanColorScheme(int index) const
Definition: PreferencesDialog.cpp:6056
rtabmap::PreferencesDialog::getOdomF2MGravitySigma
double getOdomF2MGravitySigma() const
Definition: PreferencesDialog.cpp:5827
rtabmap::PreferencesDialog::getDownsamplingStepScan
int getDownsamplingStepScan(int index) const
Definition: PreferencesDialog.cpp:6036
rtabmap::PostProcessingDialog::interSession
bool interSession() const
Definition: PostProcessingDialog.cpp:252
UVariant
Definition: UVariant.h:30
rtabmap::MainWindow::timeLimitChanged
void timeLimitChanged(float)
rtabmap::SensorEvent::data
const SensorData & data() const
Definition: SensorEvent.h:79
rtabmap::MainWindow::stateChanged
void stateChanged(MainWindow::State)
rtabmap::PreferencesDialog::kSrcFlyCapture2
@ kSrcFlyCapture2
Definition: PreferencesDialog.h:104
rtabmap::Statistics::mapCorrection
const Transform & mapCorrection() const
Definition: Statistics.h:277
rtabmap::MainWindow::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: MainWindow.cpp:5244
OctoMap.h
rtabmap::OdometryInfo::gravityPitchError
double gravityPitchError
Definition: OdometryInfo.h:123
rtabmap::MainWindow::processRtabmapGlobalPathEvent
void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &event)
Definition: MainWindow.cpp:4783
rtabmap::PreferencesDialog::saveWindowGeometry
void saveWindowGeometry(const QWidget *window)
Definition: PreferencesDialog.cpp:3848
rtabmap::MainWindow::kMonitoring
@ kMonitoring
Definition: MainWindow.h:96
iter
iterator iter(handle obj)
rtabmap::PreferencesDialog::kSrcRealSense
@ kSrcRealSense
Definition: PreferencesDialog.h:95
rtabmap::MainWindow::processRtabmapEventInit
void processRtabmapEventInit(int status, const QString &info)
Definition: MainWindow.cpp:4540
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::MainWindow::kInitializing
@ kInitializing
Definition: MainWindow.h:89
rtabmap::PreferencesDialog::loadWidgetState
void loadWidgetState(QWidget *widget)
Definition: PreferencesDialog.cpp:4024
rtabmap::PreferencesDialog::getKpMaxFeatures
int getKpMaxFeatures() const
Definition: PreferencesDialog.cpp:7105
rtabmap::RtabmapEventCmd::kCmdCleanDataBuffer
@ kCmdCleanDataBuffer
Definition: RtabmapEvent.h:71
rtabmap::PreferencesDialog::isIMUGravityShown
bool isIMUGravityShown(int index) const
Definition: PreferencesDialog.cpp:5940
rtabmap::PreferencesDialog::saveCustomConfig
void saveCustomConfig(const QString &section, const QString &key, const QString &value)
Definition: PreferencesDialog.cpp:4099
args
rtabmap::MainWindow::_odometryCorrection
Transform _odometryCorrection
Definition: MainWindow.h:400
rtabmap::CameraRealSense2::available
static bool available()
Definition: CameraRealSense2.cpp:45
rtabmap::SensorCaptureThread::setImageDecimation
void setImageDecimation(int decimation)
Definition: SensorCaptureThread.h:126
rtabmap::RtabmapEventInit::getStatus
Status getStatus() const
Definition: RtabmapEvent.h:162
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::CameraK4A::available
static bool available()
Definition: CameraK4A.cpp:42
RegistrationVis.h
util3d_registration.h
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
rtabmap::RtabmapEventCmd::kCmdClose
@ kCmdClose
Definition: RtabmapEvent.h:65
rtabmap::MainWindow::_preferencesDialog
PreferencesDialog * _preferencesDialog
Definition: MainWindow.h:349
rtabmap::MainWindow::_autoScreenCaptureRAM
bool _autoScreenCaptureRAM
Definition: MainWindow.h:423
rtabmap::MainWindow::_progressCanceled
bool _progressCanceled
Definition: MainWindow.h:431
rtabmap::Odometry::kTypeORBSLAM
@ kTypeORBSLAM
Definition: Odometry.h:52
rtabmap::MainWindow::closeDatabase
virtual bool closeDatabase()
Definition: MainWindow.cpp:5636
rtabmap::MainWindow::downloadAllClouds
virtual void downloadAllClouds()
Definition: MainWindow.cpp:7403
c_str
const char * c_str(Args &&...args)
rtabmap::MainWindow::_processingOdometry
bool _processingOdometry
Definition: MainWindow.h:402
rtabmap::PreferencesDialog::isLocalizationsCountGraphView
bool isLocalizationsCountGraphView() const
Definition: PreferencesDialog.cpp:5807
UStl.h
Wrappers of STL for convenient functions.
rtabmap::MainWindow::detectionRateChanged
void detectionRateChanged(double)
rtabmap::MainWindow::cloudViewer
rtabmap::CloudViewer * cloudViewer() const
Definition: MainWindow.h:311
rtabmap::MainWindow::selectVLP16
void selectVLP16()
Definition: MainWindow.cpp:7246
rtabmap::MainWindow::selectDepthAIOAKD
void selectDepthAIOAKD()
Definition: MainWindow.cpp:7226
rtabmap::RtabmapEvent3DMap::getPoses
const std::map< int, Transform > & getPoses() const
Definition: RtabmapEvent.h:190
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:134
rtabmap::MainWindow::setAspectRatio
void setAspectRatio(int w, int h)
Definition: MainWindow.cpp:7791
ExportCloudsDialog.h
diff
diff
rtabmap::MainWindow::exportGridMap
void exportGridMap()
Definition: MainWindow.cpp:7871
rtabmap::PreferencesDialog::createCamera
Camera * createCamera(bool useRawImages=false, bool useColor=true)
Definition: PreferencesDialog.cpp:6404
rtabmap::LoopClosureViewer::setMaxDepth
void setMaxDepth(int maxDepth)
Definition: LoopClosureViewer.h:58
rtabmap::PreferencesDialog::getSourceDistortionModel
QString getSourceDistortionModel() const
Definition: PreferencesDialog.cpp:6331
rtabmap::OdometryInfo::interval
double interval
Definition: OdometryInfo.h:114
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
rtabmap::PreferencesDialog::getSourceScanRangeMax
double getSourceScanRangeMax() const
Definition: PreferencesDialog.cpp:6383
v
Array< int, Dynamic, 1 > v
rtabmap::MainWindow::viewClouds
void viewClouds()
Definition: MainWindow.cpp:7978
UDEBUG
#define UDEBUG(...)
rtabmap::CloudViewer::getAddedFrustums
const QMap< std::string, Transform > & getAddedFrustums() const
Definition: CloudViewer.h:295
rtabmap::PreferencesDialog::getCloudMeshingAngle
double getCloudMeshingAngle() const
Definition: PreferencesDialog.cpp:5966
rtabmap::MainWindow::takeScreenshot
void takeScreenshot()
Definition: MainWindow.cpp:7786
rtabmap::MainWindow::loopClosureViewer
rtabmap::LoopClosureViewer * loopClosureViewer() const
Definition: MainWindow.h:312
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:470
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
rtabmap::PreferencesDialog::saveMainWindowState
void saveMainWindowState(const QMainWindow *mainWindow)
Definition: PreferencesDialog.cpp:3893
rtabmap::PreferencesDialog::getNoiseMinNeighbors
int getNoiseMinNeighbors() const
Definition: PreferencesDialog.cpp:5883
rtabmap::PreferencesDialog::saveWidgetState
void saveWidgetState(const QWidget *widget)
Definition: PreferencesDialog.cpp:3951
rtabmap::PreferencesDialog::getOctomapPointSize
int getOctomapPointSize() const
Definition: PreferencesDialog.cpp:5870
rtabmap::PreferencesDialog::setMonitoringState
void setMonitoringState(bool monitoringState)
Definition: PreferencesDialog.h:322
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
RegistrationIcp.h
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
rtabmap::MainWindow::selectStereoZed
void selectStereoZed()
Definition: MainWindow.cpp:7202
rtabmap::OdometryResetEvent
Definition: OdometryEvent.h:97
rtabmap::MainWindow::_lastId
int _lastId
Definition: MainWindow.h:358
UTimer
Definition: UTimer.h:46
rtabmap::MainWindow::processRtabmapLabelErrorEvent
void processRtabmapLabelErrorEvent(int id, const QString &label)
Definition: MainWindow.cpp:4842
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
ExportBundlerDialog.h
rtabmap::PreferencesDialog::notifyWhenNewGlobalPathIsReceived
bool notifyWhenNewGlobalPathIsReceived() const
Definition: PreferencesDialog.cpp:5787
rtabmap::Statistics::poses
const std::map< int, Transform > & poses() const
Definition: Statistics.h:275
rtabmap::MainWindow::_savedMaximized
bool _savedMaximized
Definition: MainWindow.h:371
rtabmap::CameraOpenni::available
static bool available()
Definition: CameraOpenni.cpp:51
rtabmap::PreferencesDialog::getLoopThr
double getLoopThr() const
Definition: PreferencesDialog.cpp:7051
rtabmap::MainWindow::processOdometry
void processOdometry(const rtabmap::OdometryEvent &odom, bool dataIgnored)
Definition: MainWindow.cpp:1041
rtabmap::MainWindow::eventFilter
virtual bool eventFilter(QObject *obj, QEvent *event)
Definition: MainWindow.cpp:5253
rtabmap::DBReader
Definition: DBReader.h:46
float
float
rtabmap::RtabmapGlobalPathEvent::getPoses
const std::vector< std::pair< int, Transform > > & getPoses() const
Definition: RtabmapEvent.h:228
rtabmap::MainWindow::noMoreImagesReceived
void noMoreImagesReceived()
initGuiResource
static void initGuiResource()
Definition: MainWindow.cpp:137
rtabmap::CloudViewer::removeElevationMap
void removeElevationMap()
Definition: CloudViewer.cpp:1727
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
rtabmap::PreferencesDialog::getSourceScanVoxelSize
double getSourceScanVoxelSize() const
Definition: PreferencesDialog.cpp:6387
rtabmap::MainWindow::_odomImageShow
bool _odomImageShow
Definition: MainWindow.h:369
rtabmap::CameraOpenNI2::available
static bool available()
Definition: CameraOpenNI2.cpp:42
rtabmap::RecoveryState
Definition: RecoveryState.h:39
rtabmap::GlobalMap::getCellSize
float getCellSize() const
Definition: GlobalMap.h:60
rtabmap::MainWindow::processCameraInfo
void processCameraInfo(const rtabmap::SensorCaptureInfo &info)
Definition: MainWindow.cpp:1017
rtabmap::PreferencesDialog::getOdomSourceDriver
PreferencesDialog::Src getOdomSourceDriver() const
Definition: PreferencesDialog.cpp:6240
rtabmap::PreferencesDialog::kSrcStereoRealSense2
@ kSrcStereoRealSense2
Definition: PreferencesDialog.h:110
rtabmap::RtabmapEventCmd::kCmdResume
@ kCmdResume
Definition: RtabmapEvent.h:76
rtabmap::PreferencesDialog::kSrcStereoZed
@ kSrcStereoZed
Definition: PreferencesDialog.h:107
rtabmap::PreferencesDialog::getCloudPointSize
int getCloudPointSize(int index) const
Definition: PreferencesDialog.cpp:6025
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
rtabmap::DatabaseViewer::openDatabase
bool openDatabase(const QString &path, const ParametersMap &overridenParameters=ParametersMap())
Definition: DatabaseViewer.cpp:820
rtabmap::PostProcessingDialog::isRefineNeighborLinks
bool isRefineNeighborLinks() const
Definition: PostProcessingDialog.cpp:257
DataRecorder.h
UThread::isRunning
bool isRunning() const
Definition: UThread.cpp:245
rtabmap::PreferencesDialog::getFloorFilteringHeight
double getFloorFilteringHeight() const
Definition: PreferencesDialog.cpp:5891
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
rtabmap::Lidar
Definition: Lidar.h:40
rtabmap::MainWindow::selectDepthAIOAKDPro
void selectDepthAIOAKDPro()
Definition: MainWindow.cpp:7236
rtabmap::MainWindow::updateGraphView
void updateGraphView()
Definition: MainWindow.cpp:4523
rtabmap::OdometryEvent::data
SensorData & data()
Definition: OdometryEvent.h:69
rtabmap::MainWindow::dataRecorder
void dataRecorder()
Definition: MainWindow.cpp:8338
rtabmap::MainWindow::MainWindow
MainWindow(PreferencesDialog *prefDialog=0, QWidget *parent=0, bool showSplashScreen=true)
Definition: MainWindow.cpp:142
rtabmap::MainWindow::cancelGoal
void cancelGoal()
Definition: MainWindow.cpp:7323
ULogger::Level
Level
Definition: ULogger.h:252
rtabmap::SensorData::gps
const GPS & gps() const
Definition: SensorData.h:291
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:886
Vector2::x
float x
rtabmap::MainWindow::getWorkingDirectory
QString getWorkingDirectory() const
Definition: MainWindow.cpp:7090
rtabmap::PreferencesDialog::getCloudMinDepth
double getCloudMinDepth(int index) const
Definition: PreferencesDialog.cpp:5992
rtabmap::OdometryInfo::timeDeskewing
float timeDeskewing
Definition: OdometryInfo.h:110
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
rtabmap::Statistics::constraints
const std::multimap< int, Link > & constraints() const
Definition: Statistics.h:276
rtabmap::MainWindow::setCloudViewer
void setCloudViewer(rtabmap::CloudViewer *cloudViewer)
Definition: MainWindow.cpp:757
false
#define false
Definition: ConvertUTF.c:56
rtabmap::MainWindow::downloadPoseGraph
virtual void downloadPoseGraph()
Definition: MainWindow.cpp:7447
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
ULogger::setPrintThreadId
static void setPrintThreadId(bool printThreadId)
Definition: ULogger.h:309
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
OccupancyGrid.h
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::MainWindow::captureScreen
QString captureScreen(bool cacheInRAM=false, bool png=true)
Definition: MainWindow.cpp:5330
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::PreferencesDialog::getNoiseRadius
double getNoiseRadius() const
Definition: PreferencesDialog.cpp:5879
rtabmap::MainWindow::setAspectRatio1080p
void setAspectRatio1080p()
Definition: MainWindow.cpp:7852
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::RtabmapEvent3DMap::getConstraints
const std::multimap< int, Link > & getConstraints() const
Definition: RtabmapEvent.h:191
rtabmap::PreferencesDialog::kSrcLidarVLP16
@ kSrcLidarVLP16
Definition: PreferencesDialog.h:123
rtabmap::SensorCaptureThread::camera
Camera * camera()
Definition: SensorCaptureThread.h:169
rtabmap::SensorData::setStereoImage
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
Definition: SensorData.cpp:304
rtabmap::CloudViewer::refreshView
void refreshView()
Definition: CloudViewer.cpp:538
rtabmap::Odometry::kTypeF2F
@ kTypeF2F
Definition: Odometry.h:48
rtabmap::PreferencesDialog::getSourceDriver
PreferencesDialog::Src getSourceDriver() const
Definition: PreferencesDialog.cpp:6192
rtabmap::PreferencesDialog::getIMUFilteringBaseFrameConversion
bool getIMUFilteringBaseFrameConversion() const
Definition: PreferencesDialog.cpp:6323
rtabmap::MainWindow::exportPoses
void exportPoses(int format)
Definition: MainWindow.cpp:6257
rtabmap::PreferencesDialog::getOctomapTreeDepth
int getOctomapTreeDepth() const
Definition: PreferencesDialog.cpp:5866
rtabmap::MainWindow::_processingStatistics
bool _processingStatistics
Definition: MainWindow.h:360
rtabmap::Statistics::loopClosureTransform
const Transform & loopClosureTransform() const
Definition: Statistics.h:278
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:904
rtabmap::MainWindow::exportBundlerFormat
void exportBundlerFormat()
Definition: MainWindow.cpp:8258
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:2359
rtabmap::OdometryInfo::guess
Transform guess
Definition: OdometryInfo.h:119
dist
dist
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::RtabmapEventCmd::kCmdRemoveLabel
@ kCmdRemoveLabel
Definition: RtabmapEvent.h:80
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1351
t
Point2 t(10, 10)
rtabmap::RtabmapEventCmd::kCmdPause
@ kCmdPause
Definition: RtabmapEvent.h:75
rtabmap::Transform::translation
Transform translation() const
Definition: Transform.cpp:203
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
exec
void exec(const char(&s)[N], object global=globals(), object local=object())
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
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:6483
rtabmap::MainWindow::exportPosesRaw
void exportPosesRaw()
Definition: MainWindow.cpp:6228
rtabmap::Statistics::odomCachePoses
const std::map< int, Transform > & odomCachePoses() const
Definition: Statistics.h:289
rtabmap::PreferencesDialog::kPanelGeneral
@ kPanelGeneral
Definition: PreferencesDialog.h:75
rtabmap::MainWindow::changeMappingMode
void changeMappingMode()
Definition: MainWindow.cpp:5325
UFile.h
rtabmap::MainWindow::_exportPosesFrame
int _exportPosesFrame
Definition: MainWindow.h:420
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::RtabmapGlobalPathEvent
Definition: RtabmapEvent.h:201
rtabmap::PreferencesDialog::getGeneralLoggerPrintThreadId
bool getGeneralLoggerPrintThreadId() const
Definition: PreferencesDialog.cpp:5747
MultiSessionLocWidget.h
rtabmap::PreferencesDialog::kSrcOpenNI_CV
@ kSrcOpenNI_CV
Definition: PreferencesDialog.h:91
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::LocalGridCache::size
size_t size() const
Definition: LocalGrid.h:76
rtabmap::MainWindow::selectOpenni
void selectOpenni()
Definition: MainWindow.cpp:7134
UEventsManager.h
rtabmap::SensorCaptureThread::setHistogramMethod
void setHistogramMethod(int histogramMethod)
Definition: SensorCaptureThread.h:127
rtabmap::PreferencesDialog::kSrcUndef
@ kSrcUndef
Definition: PreferencesDialog.h:86
rtabmap::ExportBundlerDialog::setWorkingDirectory
void setWorkingDirectory(const QString &path)
Definition: ExportBundlerDialog.cpp:127
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
rtabmap::MainWindow::_currentMapIds
std::map< int, int > _currentMapIds
Definition: MainWindow.h:382
rtabmap::MainWindow::updateElapsedTime
void updateElapsedTime()
Definition: MainWindow.cpp:7582
rtabmap::MainWindow::printLoopClosureIds
void printLoopClosureIds()
Definition: MainWindow.cpp:6177
rtabmap::PreferencesDialog::kSrcFreenect
@ kSrcFreenect
Definition: PreferencesDialog.h:90
UERROR
#define UERROR(...)
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_CORE_EXPORT 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:3035
rtabmap::Signature::removeAllWords
void removeAllWords()
Definition: Signature.cpp:346
rtabmap::MainWindow::handleEvent
virtual bool handleEvent(UEvent *anEvent)
Definition: MainWindow.cpp:876
DatabaseViewer.h
rtabmap::OdometryInfo::transformGroundTruth
Transform transformGroundTruth
Definition: OdometryInfo.h:117
rtabmap::OdometryInfo::gravityRollError
double gravityRollError
Definition: OdometryInfo.h:122
rtabmap::MainWindow::State
State
Definition: MainWindow.h:87
trace.model
model
Definition: trace.py:4
rtabmap::PostProcessingDialog::sbaRematchFeatures
bool sbaRematchFeatures() const
Definition: PostProcessingDialog.cpp:284
UCvMat2QImageThread
Definition: UCv2Qt.h:263
file
file
rtabmap::Statistics::weights
const std::map< int, int > & weights() const
Definition: Statistics.h:281
rtabmap::MainWindow::selectOpenni2
void selectOpenni2()
Definition: MainWindow.cpp:7154
rtabmap::databaseRecovery
bool RTABMAP_CORE_EXPORT databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
rtabmap::MainWindow::setAspectRatio16_9
void setAspectRatio16_9()
Definition: MainWindow.cpp:7817
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::MainWindow::_odomImageDepthShow
bool _odomImageDepthShow
Definition: MainWindow.h:370
rtabmap::PreferencesDialog::getGeneralLoggerEventLevel
int getGeneralLoggerEventLevel() const
Definition: PreferencesDialog.cpp:5731
rtabmap::MainWindow::_currentLinksMap
std::multimap< int, Link > _currentLinksMap
Definition: MainWindow.h:381
rtabmap::MainWindow::_logEventTime
QElapsedTimer * _logEventTime
Definition: MainWindow.h:406
rtabmap::MainWindow::deleteMemory
virtual void deleteMemory()
Definition: MainWindow.cpp:7057
rtabmap::PreferencesDialog::getSourceScanRangeMin
double getSourceScanRangeMin() const
Definition: PreferencesDialog.cpp:6379
rtabmap::PdfPlotCurve::clear
virtual void clear()
Definition: PdfPlot.cpp:131
rtabmap::RtabmapEventInit::kInitializing
@ kInitializing
Definition: RtabmapEvent.h:140
rtabmap::PreferencesDialog::kSrcOpenNI_CV_ASUS
@ kSrcOpenNI_CV_ASUS
Definition: PreferencesDialog.h:92
rtabmap::MainWindow::mappingModeChanged
void mappingModeChanged(bool)
rtabmap::RtabmapEventInit::getInfo
const std::string & getInfo() const
Definition: RtabmapEvent.h:163
value
value
rtabmap::Odometry::kTypeF2M
@ kTypeF2M
Definition: Odometry.h:47
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:564
rtabmap::MainWindow::_cachedEmptyClouds
std::set< int > _cachedEmptyClouds
Definition: MainWindow.h:386
i
int i
rtabmap::PreferencesDialog::isSourceMirroring
bool isSourceMirroring() const
Definition: PreferencesDialog.cpp:6167
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:2750
rtabmap::SensorEvent
Definition: SensorEvent.h:37
rtabmap::SensorCaptureThread::enableBilateralFiltering
void enableBilateralFiltering(float sigmaS, float sigmaR)
Definition: SensorCaptureThread.cpp:216
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: DBDriver.cpp:925
text
text
rtabmap::MainWindow::_lastIds
QSet< int > _lastIds
Definition: MainWindow.h:357
rtabmap::PreferencesDialog::isDepthFilteringAvailable
bool isDepthFilteringAvailable() const
Definition: PreferencesDialog.cpp:6327
rtabmap::RtabmapEventCmd::kCmdDumpPrediction
@ kCmdDumpPrediction
Definition: RtabmapEvent.h:68
rtabmap::Statistics::stamp
double stamp() const
Definition: Statistics.h:270
rtabmap::SensorCaptureInfo::odomCovariance
cv::Mat odomCovariance
Definition: SensorCaptureInfo.h:75
rtabmap::PreferencesDialog::selectSourceDriver
void selectSourceDriver(Src src, int variant=0)
Definition: PreferencesDialog.cpp:4187
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::PreferencesDialog::isWordsCountGraphView
bool isWordsCountGraphView() const
Definition: PreferencesDialog.cpp:5803
rtabmap::Statistics::refImageId
int refImageId() const
Definition: Statistics.h:264
rtabmap::MainWindow::exportOctomap
void exportOctomap()
Definition: MainWindow.cpp:8016
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::CloudViewer::clearTrajectory
void clearTrajectory()
Definition: CloudViewer.cpp:2435
rtabmap::MainWindow::depthCalibration
void depthCalibration()
Definition: MainWindow.cpp:7041
rtabmap::PreferencesDialog::kSrcK4W2
@ kSrcK4W2
Definition: PreferencesDialog.h:97
rtabmap::PreferencesDialog::getScanMinRange
double getScanMinRange(int index) const
Definition: PreferencesDialog.cpp:6046
rtabmap::Signature
Definition: Signature.h:48
Recovery.h
rtabmap::RtabmapEvent::getStats
const Statistics & getStats() const
Definition: RtabmapEvent.h:50
rtabmap::SensorData::depthOrRightCompressed
const cv::Mat & depthOrRightCompressed() const
Definition: SensorData.h:180
rtabmap::MainWindow::clearTheCache
virtual void clearTheCache()
Definition: MainWindow.cpp:7502
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:1087
rtabmap::PreferencesDialog::kSrcDC1394
@ kSrcDC1394
Definition: PreferencesDialog.h:103
rtabmap::Optimizer::getConnectedGraph
void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut) const
Definition: Optimizer.cpp:188
rtabmap::SensorCaptureThread::setFrameRate
void setFrameRate(float frameRate)
Definition: SensorCaptureThread.cpp:184
rtabmap::PreferencesDialog::isSourceScanDeskewing
bool isSourceScanDeskewing() const
Definition: PreferencesDialog.cpp:6371
rtabmap::OdometryInfo::localBundleOutliers
int localBundleOutliers
Definition: OdometryInfo.h:104
rtabmap::MainWindow::kApplicationClosing
@ kApplicationClosing
Definition: MainWindow.h:91
rtabmap::SensorCaptureThread::setOdomAsGroundTruth
void setOdomAsGroundTruth(bool enabled)
Definition: SensorCaptureThread.h:132
rtabmap::MainWindow::exportPosesRGBDSLAMMotionCapture
void exportPosesRGBDSLAMMotionCapture()
Definition: MainWindow.cpp:6232
rtabmap::PreferencesDialog::getGridMapShown
bool getGridMapShown() const
Definition: PreferencesDialog.cpp:6120
rtabmap::ProgressDialog::maximumSteps
int maximumSteps() const
Definition: ProgressDialog.cpp:129
rtabmap::MainWindow::_lastOdomPose
Transform _lastOdomPose
Definition: MainWindow.h:401
rtabmap::SensorData::groundTruth
const Transform & groundTruth() const
Definition: SensorData.h:284
rtabmap::MainWindow::_loopClosureIds
QVector< int > _loopClosureIds
Definition: MainWindow.h:428
rtabmap::PreferencesDialog::isCacheSavedInFigures
bool isCacheSavedInFigures() const
Definition: PreferencesDialog.cpp:5783
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28
rtabmap::RegistrationInfo::icpStructuralDistribution
float icpStructuralDistribution
Definition: RegistrationInfo.h:94
rtabmap::MainWindow::_waypoints
QStringList _waypoints
Definition: MainWindow.h:372
msg
msg
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:1205
rtabmap::PreferencesDialog::isOdomOnlyInliersShown
bool isOdomOnlyInliersShown() const
Definition: PreferencesDialog.cpp:5795
rtabmap::MainWindow::updateNodeVisibility
void updateNodeVisibility(int, bool)
Definition: MainWindow.cpp:4465
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
rtabmap::PreferencesDialog::isSubtractFiltering
bool isSubtractFiltering() const
Definition: PreferencesDialog.cpp:6092
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
R
R
rtabmap::MainWindow::setAspectRatioCustom
void setAspectRatioCustom()
Definition: MainWindow.cpp:7857
rtabmap::PreferencesDialog::kSrcSeerSense
@ kSrcSeerSense
Definition: PreferencesDialog.h:100

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12