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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:56