MainWindow.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/MainWindow.h"
29 
30 #include "ui_mainWindow.h"
31 
32 #include "rtabmap/core/CameraRGB.h"
33 #include "rtabmap/core/CameraStereo.h"
34 #include "rtabmap/core/CameraThread.h"
35 #include "rtabmap/core/IMUThread.h"
36 #include "rtabmap/core/CameraEvent.h"
37 #include "rtabmap/core/DBReader.h"
38 #include "rtabmap/core/Parameters.h"
39 #include "rtabmap/core/ParamEvent.h"
40 #include "rtabmap/core/Signature.h"
41 #include "rtabmap/core/Memory.h"
42 #include "rtabmap/core/DBDriver.h"
43 #include "rtabmap/core/RegistrationVis.h"
44 #include "rtabmap/core/OccupancyGrid.h"
45 #include "rtabmap/core/GainCompensator.h"
46 #include "rtabmap/core/Recovery.h"
47 #include "rtabmap/core/util2d.h"
48 
49 #include "rtabmap/gui/ImageView.h"
50 #include "rtabmap/gui/KeypointItem.h"
51 #include "rtabmap/gui/DataRecorder.h"
52 #include "rtabmap/gui/DatabaseViewer.h"
53 #include "rtabmap/gui/PdfPlot.h"
54 #include "rtabmap/gui/StatsToolBox.h"
55 #include "rtabmap/gui/ProgressDialog.h"
56 #include "rtabmap/gui/CloudViewer.h"
57 #include "rtabmap/gui/LoopClosureViewer.h"
58 #include "rtabmap/gui/ExportCloudsDialog.h"
59 #include "rtabmap/gui/ExportBundlerDialog.h"
60 #include "rtabmap/gui/AboutDialog.h"
61 #include "rtabmap/gui/PostProcessingDialog.h"
62 #include "rtabmap/gui/DepthCalibrationDialog.h"
63 #include "rtabmap/gui/RecoveryState.h"
64 
65 #include <rtabmap/utilite/UStl.h>
66 #include <rtabmap/utilite/ULogger.h>
67 #include <rtabmap/utilite/UEventsManager.h>
68 #include <rtabmap/utilite/UFile.h>
69 #include <rtabmap/utilite/UConversion.h>
70 #include "rtabmap/utilite/UPlot.h"
71 #include "rtabmap/utilite/UCv2Qt.h"
72 
73 #include <QtGui/QCloseEvent>
74 #include <QtGui/QPixmap>
75 #include <QtCore/QDir>
76 #include <QtCore/QFile>
77 #include <QtCore/QTextStream>
78 #include <QtCore/QFileInfo>
79 #include <QMessageBox>
80 #include <QFileDialog>
81 #include <QGraphicsEllipseItem>
82 #include <QDockWidget>
83 #include <QtCore/QBuffer>
84 #include <QtCore/QTimer>
85 #include <QtCore/QTime>
86 #include <QActionGroup>
87 #include <QtGui/QDesktopServices>
88 #include <QtCore/QStringList>
89 #include <QtCore/QProcess>
90 #include <QSplashScreen>
91 #include <QInputDialog>
92 #include <QToolButton>
93 
94 //RGB-D stuff
95 #include "rtabmap/core/CameraRGBD.h"
96 #include "rtabmap/core/Odometry.h"
97 #include "rtabmap/core/OdometryThread.h"
98 #include "rtabmap/core/OdometryEvent.h"
99 #include "rtabmap/core/util3d.h"
100 #include "rtabmap/core/util3d_transforms.h"
101 #include "rtabmap/core/util3d_filtering.h"
102 #include "rtabmap/core/util3d_mapping.h"
103 #include "rtabmap/core/util3d_surface.h"
104 #include "rtabmap/core/util3d_registration.h"
105 #include "rtabmap/core/Optimizer.h"
106 #include "rtabmap/core/optimizer/OptimizerCVSBA.h"
107 #include "rtabmap/core/Graph.h"
108 #include "rtabmap/core/RegistrationIcp.h"
109 #include <pcl/visualization/cloud_viewer.h>
110 #include <pcl/common/transforms.h>
111 #include <pcl/common/common.h>
112 #include <pcl/io/pcd_io.h>
113 #include <pcl/io/ply_io.h>
114 #include <pcl/filters/filter.h>
115 #include <pcl/search/kdtree.h>
116 
117 #ifdef RTABMAP_OCTOMAP
118 #include <rtabmap/core/OctoMap.h>
119 #endif
120 
121 #ifdef HAVE_OPENCV_ARUCO
122 #include <opencv2/aruco.hpp>
123 #endif
124 
125 #define LOG_FILE_NAME "LogRtabmap.txt"
126 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
127 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m"
128 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m"
129 
130 using namespace rtabmap;
131 
132 inline static void initGuiResource() { Q_INIT_RESOURCE(GuiLib); }
133 
134 
135 namespace rtabmap {
136 
137 MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool showSplashScreen) :
138  QMainWindow(parent),
139  _ui(0),
140  _state(kIdle),
141  _camera(0),
142  _odomThread(0),
143  _imuThread(0),
144  _preferencesDialog(0),
145  _aboutDialog(0),
146  _exportCloudsDialog(0),
147  _exportBundlerDialog(0),
148  _dataRecorder(0),
149  _lastId(0),
150  _firstStamp(0.0f),
151  _processingStatistics(false),
152  _processingDownloadedMap(false),
153  _recovering(false),
154  _odometryReceived(false),
155  _newDatabasePath(""),
156  _newDatabasePathOutput(""),
157  _openedDatabasePath(""),
158  _databaseUpdated(false),
159  _odomImageShow(true),
160  _odomImageDepthShow(false),
161  _savedMaximized(false),
162  _waypointsIndex(0),
163  _cachedMemoryUsage(0),
164  _createdCloudsMemoryUsage(0),
165  _occupancyGrid(0),
166  _octomap(0),
167  _odometryCorrection(Transform::getIdentity()),
168  _processingOdometry(false),
169  _oneSecondTimer(0),
170  _elapsedTime(0),
171  _posteriorCurve(0),
172  _likelihoodCurve(0),
173  _rawLikelihoodCurve(0),
174  _exportPosesFrame(0),
175  _autoScreenCaptureOdomSync(false),
176  _autoScreenCaptureRAM(false),
177  _autoScreenCapturePNG(false),
178  _firstCall(true),
179  _progressCanceled(false)
180 {
181  ULogger::registerCurrentThread("MainWindow");
182  UDEBUG("");
183 
184  initGuiResource();
185 
186  QSplashScreen * splash = 0;
187  if (showSplashScreen)
188  {
189  QPixmap pixmap(":images/RTAB-Map.png");
190  splash = new QSplashScreen(pixmap);
191  splash->show();
192  splash->showMessage(tr("Loading..."));
193  QApplication::processEvents();
194  }
195 
196  // Create dialogs
197  _aboutDialog = new AboutDialog(this);
198  _aboutDialog->setObjectName("AboutDialog");
199  _exportCloudsDialog = new ExportCloudsDialog(this);
200  _exportCloudsDialog->setObjectName("ExportCloudsDialog");
201  _exportBundlerDialog = new ExportBundlerDialog(this);
202  _exportBundlerDialog->setObjectName("ExportBundlerDialog");
203  _postProcessingDialog = new PostProcessingDialog(this);
204  _postProcessingDialog->setObjectName("PostProcessingDialog");
205  _depthCalibrationDialog = new DepthCalibrationDialog(this);
206  _depthCalibrationDialog->setObjectName("DepthCalibrationDialog");
207 
208  _ui = new Ui_mainWindow();
209  UDEBUG("Setup ui...");
210  _ui->setupUi(this);
211 
212  // Add cloud viewers
213  // Note that we add them here manually because there is a crash issue
214  // when adding them in a DockWidget of the *.ui file. The cloud viewer is
215  // created in a widget which is not yet linked to main window when the CloudViewer constructor
216  // is called (see order in generated ui file). VTK needs to get the top
217  // level window at the time CloudViewer is created, otherwise it may crash on some systems.
218  _cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
219  _cloudViewer->setObjectName("widget_cloudViewer");
220  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
221  _loopClosureViewer = new LoopClosureViewer(_ui->layout_loopClosureViewer);
222  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
223  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
224  UDEBUG("Setup ui... end");
225 
226  QString title("RTAB-Map[*]");
227  this->setWindowTitle(title);
228  this->setWindowIconText(tr("RTAB-Map"));
229  this->setObjectName("MainWindow");
230 
231  //Setup dock widgets position if it is the first time the application is started.
232  setDefaultViews();
233 
234  _ui->widget_mainWindow->setVisible(false);
235 
236  if(prefDialog)
237  {
238  _preferencesDialog = prefDialog;
239  _preferencesDialog->setParent(this, Qt::Dialog);
240  }
241  else // Default dialog
242  {
243  _preferencesDialog = new PreferencesDialog(this);
244  }
245 
246  _preferencesDialog->setObjectName("PreferencesDialog");
247  _preferencesDialog->init();
248 
249  // Restore window geometry
250  bool statusBarShown = false;
251  _preferencesDialog->loadMainWindowState(this, _savedMaximized, statusBarShown);
252  _preferencesDialog->loadWindowGeometry(_preferencesDialog);
253  _preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
254  _preferencesDialog->loadWindowGeometry(_exportBundlerDialog);
255  _preferencesDialog->loadWindowGeometry(_postProcessingDialog);
256  _preferencesDialog->loadWindowGeometry(_depthCalibrationDialog);
257  _preferencesDialog->loadWindowGeometry(_aboutDialog);
258  setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
259 
260  ParametersMap parameters = _preferencesDialog->getAllParameters();
261  _occupancyGrid = new OccupancyGrid();
262 #ifdef RTABMAP_OCTOMAP
263  _octomap = new OctoMap(parameters);
264 #endif
265 
266  // Timer
267  _oneSecondTimer = new QTimer(this);
268  _oneSecondTimer->setInterval(1000);
269  _elapsedTime = new QTime();
270  _ui->label_elapsedTime->setText("00:00:00");
271  connect(_oneSecondTimer, SIGNAL(timeout()), this, SLOT(updateElapsedTime()));
272  _logEventTime = new QTime();
273  _logEventTime->start();
274 
275  //Graphics scenes
276  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
277  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
278  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
279  _ui->imageView_odometry->setAlpha(200);
280  _preferencesDialog->loadWidgetState(_ui->imageView_source);
281  _preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
282  _preferencesDialog->loadWidgetState(_ui->imageView_odometry);
283  _preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
284 
285  _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
286  _ui->posteriorPlot->addCurve(_posteriorCurve, false);
287  _ui->posteriorPlot->showLegend(false);
288  _ui->posteriorPlot->setFixedYAxis(0,1);
289  UPlotCurveThreshold * tc;
290  tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
291  connect(this, SIGNAL(loopClosureThrChanged(qreal)), tc, SLOT(setThreshold(qreal)));
292 
293  _likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
294  _ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
295  _ui->likelihoodPlot->showLegend(false);
296 
297  _rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
298  _ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
299  _ui->rawLikelihoodPlot->showLegend(false);
300 
301  _progressDialog = new ProgressDialog(this);
302  _progressDialog->setMinimumWidth(800);
303  connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancelProgress()));
304 
305  connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
306 
307  //connect stuff
308  connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
309  qRegisterMetaType<MainWindow::State>("MainWindow::State");
310  connect(this, SIGNAL(stateChanged(MainWindow::State)), this, SLOT(changeState(MainWindow::State)));
311  connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
312  qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
313  connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
314  qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
315  connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
316  connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
317  connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
318 
319  // Dock Widget view actions (Menu->Window)
320  _ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
321  _ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
322  _ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
323  _ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
324  _ui->menuShow_view->addAction(_ui->dockWidget_statsV2->toggleViewAction());
325  _ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
326  _ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
327  _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
328  _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
329  _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
330  _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
331  _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
332  _ui->toolBar->setWindowTitle(tr("File toolbar"));
333  _ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
334  _ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
335  QAction * a = _ui->menuShow_view->addAction("Progress dialog");
336  a->setCheckable(false);
337  connect(a, SIGNAL(triggered(bool)), _progressDialog, SLOT(show()));
338  QAction * statusBarAction = _ui->menuShow_view->addAction("Status bar");
339  statusBarAction->setCheckable(true);
340  statusBarAction->setChecked(statusBarShown);
341  connect(statusBarAction, SIGNAL(toggled(bool)), this->statusBar(), SLOT(setVisible(bool)));
342 
343  // connect actions with custom slots
344  connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
345  connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
346  connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
347  connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
348  connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
349  connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
350  connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
351  connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
352  connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
353  connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
354  connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
355  connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
356  connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
357  connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
358  connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
359  connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
360  connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
361  connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
362  connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
363  connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
364  connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
365  connect(_ui->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
366  connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
367  connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
368  connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
369  connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
370  connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
371  connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
372  connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
373  connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
374  connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
375  connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
376  connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
377  connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
378  connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
379  connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
380  connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
381  connect(_ui->action240p, SIGNAL(triggered()), this, SLOT(setAspectRatio240p()));
382  connect(_ui->action360p, SIGNAL(triggered()), this, SLOT(setAspectRatio360p()));
383  connect(_ui->action480p, SIGNAL(triggered()), this, SLOT(setAspectRatio480p()));
384  connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
385  connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
386  connect(_ui->actionCustom, SIGNAL(triggered()), this, SLOT(setAspectRatioCustom()));
387  connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
388  connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
389  connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages()));
390  connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat()));
391  connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap()));
392  connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
393  connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
394  connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
395  connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
396  connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
397  connect(_ui->actionDepth_Calibration, SIGNAL(triggered()), this, SLOT(depthCalibration()));
398 
399  _ui->actionPause->setShortcut(Qt::Key_Space);
400  _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
401  // Qt5 issue, we should explicitly add actions not in
402  // menu bar to have shortcut working
403  this->addAction(_ui->actionSave_GUI_config);
404  _ui->actionReset_Odometry->setEnabled(false);
405  _ui->actionPost_processing->setEnabled(false);
406  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
407 
408  QToolButton* toolButton = new QToolButton(this);
409  toolButton->setMenu(_ui->menuSelect_source);
410  toolButton->setPopupMode(QToolButton::InstantPopup);
411  toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
412  toolButton->setToolTip("Select sensor driver");
413  _ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
414 
415 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
416  connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
417 #else
418  _ui->menuEdit->removeAction(_ui->actionOpen_working_directory);
419 #endif
420 
421  //Settings menu
422  connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
423  connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
424  connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
425  connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
426  connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
427  connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
428  connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
429  connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
430  connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
431  connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
432  connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
433  connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
434  connect(_ui->actionKinect_for_Azure, SIGNAL(triggered()), this, SLOT(selectK4A()));
435  connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
436  connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
437  connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
438  connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
439  connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
440  connect(_ui->actionRealSense2_L515, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
441  connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
442  connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
443  connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
444  connect(_ui->actionStereoTara, SIGNAL(triggered()), this, SLOT(selectStereoTara()));
445  connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
446  connect(_ui->actionRealSense2_T265, SIGNAL(triggered()), this, SLOT(selectRealSense2Stereo()));
447  connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS()));
448  _ui->actionFreenect->setEnabled(CameraFreenect::available());
449  _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
450  _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
451  _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
452  _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
453  _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
454  _ui->actionFreenect2->setEnabled(CameraFreenect2::available());
455  _ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
456  _ui->actionKinect_for_Azure->setEnabled(CameraK4A::available());
457  _ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
458  _ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
459  _ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
460  _ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
461  _ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
462  _ui->actionRealSense2_L515->setEnabled(CameraRealSense2::available());
463  _ui->actionRealSense2_T265->setEnabled(CameraRealSense2::available());
464  _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
465  _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
466  _ui->actionStereoZed->setEnabled(CameraStereoZed::available());
467  _ui->actionStereoTara->setEnabled(CameraStereoTara::available());
468  _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available());
469  this->updateSelectSourceMenu();
470 
471  connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
472 
473  QActionGroup * modeGrp = new QActionGroup(this);
474  modeGrp->addAction(_ui->actionSLAM_mode);
475  modeGrp->addAction(_ui->actionLocalization_mode);
476  _ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
477  _ui->actionLocalization_mode->setChecked(!_preferencesDialog->isSLAMMode());
478  connect(_ui->actionSLAM_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
479  connect(_ui->actionLocalization_mode, SIGNAL(triggered()), this, SLOT(changeMappingMode()));
480  connect(this, SIGNAL(mappingModeChanged(bool)), _preferencesDialog, SLOT(setSLAMMode(bool)));
481 
482  // Settings changed
483  qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>("PreferencesDialog::PANEL_FLAGS");
484  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
485  qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
486  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
487  // config GUI modified
488  connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(configGUIModified()));
489  if(prefDialog == 0)
490  {
491  connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(configGUIModified()));
492  }
493  connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
494  connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
495  connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
496  connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
497  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
498  connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
499  connect(_exportBundlerDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
500  connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
501  connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
502  connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
503  connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
504  connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
505  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
506  for(int i=0; i<dockWidgets.size(); ++i)
507  {
508  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
509  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
510  }
511  connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
512  // catch resize events
513  _ui->dockWidget_posterior->installEventFilter(this);
514  _ui->dockWidget_likelihood->installEventFilter(this);
515  _ui->dockWidget_rawlikelihood->installEventFilter(this);
516  _ui->dockWidget_statsV2->installEventFilter(this);
517  _ui->dockWidget_console->installEventFilter(this);
518  _ui->dockWidget_loopClosureViewer->installEventFilter(this);
519  _ui->dockWidget_mapVisibility->installEventFilter(this);
520  _ui->dockWidget_graphViewer->installEventFilter(this);
521  _ui->dockWidget_odometry->installEventFilter(this);
522  _ui->dockWidget_cloudViewer->installEventFilter(this);
523  _ui->dockWidget_imageView->installEventFilter(this);
524 
525  // more connects...
526  _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
527  _ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
528  _ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
529  connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
530  connect(_ui->doubleSpinBox_stats_detectionRate, SIGNAL(editingFinished()), this, SLOT(changeDetectionRateSetting()));
531  connect(_ui->doubleSpinBox_stats_timeLimit, SIGNAL(editingFinished()), this, SLOT(changeTimeLimitSetting()));
532  connect(this, SIGNAL(imgRateChanged(double)), _preferencesDialog, SLOT(setInputRate(double)));
533  connect(this, SIGNAL(detectionRateChanged(double)), _preferencesDialog, SLOT(setDetectionRate(double)));
534  connect(this, SIGNAL(timeLimitChanged(float)), _preferencesDialog, SLOT(setTimeLimit(float)));
535 
536  // Statistics from the detector
537  qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
538  connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
539 
540  qRegisterMetaType<rtabmap::CameraInfo>("rtabmap::CameraInfo");
541  connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo)));
542 
543  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
544  connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool)));
545 
546  connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
547 
548  // Apply state
549  this->changeState(kIdle);
550  this->applyPrefSettings(PreferencesDialog::kPanelAll);
551 
552  _ui->statsToolBox->setNewFigureMaxItems(50);
553  _ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
554  _ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
555  _exportBundlerDialog->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
556  _cloudViewer->setBackfaceCulling(true, false);
557  _preferencesDialog->loadWidgetState(_cloudViewer);
558 
559  //dialog states
560  _preferencesDialog->loadWidgetState(_exportCloudsDialog);
561  _preferencesDialog->loadWidgetState(_exportBundlerDialog);
562  _preferencesDialog->loadWidgetState(_postProcessingDialog);
563  _preferencesDialog->loadWidgetState(_depthCalibrationDialog);
564 
565  if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
566  {
567  const std::map<std::string, float> & statistics = Statistics::defaultData();
568  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
569  {
570  // Don't add Gt panels yet if we don't know if we will receive Gt values.
571  if(!QString((*iter).first.c_str()).contains("Gt/"))
572  {
573  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
574  }
575  }
576  }
577  // Specific MainWindow
578  _ui->statsToolBox->updateStat("Planning/From/", false);
579  _ui->statsToolBox->updateStat("Planning/Time/ms", false);
580  _ui->statsToolBox->updateStat("Planning/Goal/", false);
581  _ui->statsToolBox->updateStat("Planning/Poses/", false);
582  _ui->statsToolBox->updateStat("Planning/Length/m", false);
583 
584  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false);
585  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false);
586  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false);
587  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false);
588  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false);
589 
590  _ui->statsToolBox->updateStat("Odometry/ID/", false);
591  _ui->statsToolBox->updateStat("Odometry/Features/", false);
592  _ui->statsToolBox->updateStat("Odometry/Matches/", false);
593  _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
594  _ui->statsToolBox->updateStat("Odometry/Inliers/", false);
595  _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", false);
596  _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", false);
597  _ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
598  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
599  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
600  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
601  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
602  _ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
603  _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
604  _ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
605  _ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
606  _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
607  _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
608  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
609  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
610  _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false);
611  _ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", false);
612  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
613  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
614  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
615  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
616  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
617  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
618  _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
619  _ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
620  _ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
621  _ui->statsToolBox->updateStat("Odometry/Speed/mph", false);
622  _ui->statsToolBox->updateStat("Odometry/Speed/mps", false);
623  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", false);
624  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", false);
625  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", false);
626  _ui->statsToolBox->updateStat("Odometry/Distance/m", false);
627  _ui->statsToolBox->updateStat("Odometry/T/m", false);
628  _ui->statsToolBox->updateStat("Odometry/Tx/m", false);
629  _ui->statsToolBox->updateStat("Odometry/Ty/m", false);
630  _ui->statsToolBox->updateStat("Odometry/Tz/m", false);
631  _ui->statsToolBox->updateStat("Odometry/Troll/deg", false);
632  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", false);
633  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", false);
634  _ui->statsToolBox->updateStat("Odometry/Px/m", false);
635  _ui->statsToolBox->updateStat("Odometry/Py/m", false);
636  _ui->statsToolBox->updateStat("Odometry/Pz/m", false);
637  _ui->statsToolBox->updateStat("Odometry/Proll/deg", false);
638  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", false);
639  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", false);
640 
641  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false);
642  _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false);
643  _ui->statsToolBox->updateStat("GUI/Graph Update/ms", false);
644 #ifdef RTABMAP_OCTOMAP
645  _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
646  _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
647 #endif
648  _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
649  _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
650  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
651  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
652  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
653 #ifdef RTABMAP_OCTOMAP
654  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", false);
655 #endif
656 
657  this->loadFigures();
658  connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
659 
660  // update loop closure viewer parameters
661  _loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
662  _loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
663 
664  if (splash)
665  {
666  splash->close();
667  delete splash;
668  }
669 
670  this->setFocus();
671 
672  UDEBUG("");
673 }
674 
675 MainWindow::~MainWindow()
676 {
677  UDEBUG("");
678  this->stopDetection();
679  delete _ui;
680  delete _elapsedTime;
681 #ifdef RTABMAP_OCTOMAP
682  delete _octomap;
683 #endif
684  delete _occupancyGrid;
685  UDEBUG("");
686 }
687 
688 void MainWindow::setupMainLayout(bool vertical)
689 {
690  if(vertical)
691  {
692  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
693  }
694  else if(!vertical)
695  {
696  qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
697  }
698 }
699 
700 void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
701 {
702  UASSERT(cloudViewer);
703  delete _cloudViewer;
704  _cloudViewer = cloudViewer;
705  _cloudViewer->setParent(_ui->layout_cloudViewer);
706  _cloudViewer->setObjectName("widget_cloudViewer");
707  _ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
708 
709  _cloudViewer->setBackfaceCulling(true, false);
710  _preferencesDialog->loadWidgetState(_cloudViewer);
711 
712  connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
713 }
714 void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
715 {
716  UASSERT(loopClosureViewer);
717  delete _loopClosureViewer;
718  _loopClosureViewer = loopClosureViewer;
719  _loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
720  _loopClosureViewer->setObjectName("widget_loopClosureViewer");
721  _ui->layout_loopClosureViewer->layout()->addWidget(_loopClosureViewer);
722 }
723 
724 void MainWindow::closeEvent(QCloseEvent* event)
725 {
726  // Try to close all children
727  /*QList<QMainWindow *> windows = this->findChildren<QMainWindow *>();
728  for(int i=0; i<windows.size(); i++) {
729  if(!windows[i]->close()) {
730  event->setAccepted(false);
731  return;
732  }
733  }*/
734  UDEBUG("");
735  bool processStopped = true;
736  if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
737  {
738  this->stopDetection();
739  if(_state == kInitialized)
740  {
741  if(this->closeDatabase())
742  {
743  this->changeState(kApplicationClosing);
744  }
745  }
746  if(_state != kIdle)
747  {
748  processStopped = false;
749  }
750  }
751 
752  if(processStopped)
753  {
754  //write settings before quit?
755  bool save = false;
756  if(this->isWindowModified())
757  {
758  QMessageBox::Button b=QMessageBox::question(this,
759  tr("RTAB-Map"),
760  tr("There are unsaved changed settings. Save them?"),
761  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
762  if(b == QMessageBox::Save)
763  {
764  save = true;
765  }
766  else if(b != QMessageBox::Discard)
767  {
768  event->ignore();
769  return;
770  }
771  }
772 
773  if(save)
774  {
775  saveConfigGUI();
776  }
777 
778  _ui->statsToolBox->closeFigures();
779 
780  _ui->dockWidget_imageView->close();
781  _ui->dockWidget_likelihood->close();
782  _ui->dockWidget_rawlikelihood->close();
783  _ui->dockWidget_posterior->close();
784  _ui->dockWidget_statsV2->close();
785  _ui->dockWidget_console->close();
786  _ui->dockWidget_cloudViewer->close();
787  _ui->dockWidget_loopClosureViewer->close();
788  _ui->dockWidget_mapVisibility->close();
789  _ui->dockWidget_graphViewer->close();
790  _ui->dockWidget_odometry->close();
791 
792  if(_camera)
793  {
794  UERROR("Camera must be already deleted here!");
795  delete _camera;
796  _camera = 0;
797  if(_imuThread)
798  {
799  delete _imuThread;
800  _imuThread = 0;
801  }
802  }
803  if(_odomThread)
804  {
805  UERROR("OdomThread must be already deleted here!");
806  delete _odomThread;
807  _odomThread = 0;
808  }
809  event->accept();
810  }
811  else
812  {
813  event->ignore();
814  }
815  UDEBUG("");
816 }
817 
818 bool MainWindow::handleEvent(UEvent* anEvent)
819 {
820  if(anEvent->getClassName().compare("IMUEvent") == 0)
821  {
822  // IMU events are published at high frequency, early exit
823  return false;
824  }
825  else if(anEvent->getClassName().compare("RtabmapEvent") == 0)
826  {
827  RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
828  Statistics stats = rtabmapEvent->getStats();
829  int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
830  int proximityClosureId = int(uValue(stats.data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
831  bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
832  float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
833  if((stats.loopClosureId() > 0 &&
834  _ui->actionPause_on_match->isChecked())
835  ||
836  (stats.loopClosureId() == 0 &&
837  highestHypothesisId > 0 &&
838  highestHypothesisValue >= _preferencesDialog->getLoopThr() &&
839  _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
840  rejectedHyp)
841  ||
842  (proximityClosureId > 0 &&
843  _ui->actionPause_on_local_loop_detection->isChecked()))
844  {
845  if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
846  {
847  if(_preferencesDialog->beepOnPause())
848  {
849  QMetaObject::invokeMethod(this, "beep");
850  }
851  this->pauseDetection();
852  }
853  }
854 
855  if(!_processingDownloadedMap)
856  {
857  _processingStatistics = true;
858  Q_EMIT statsReceived(stats);
859  }
860  }
861  else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
862  {
863  if(!_recovering)
864  {
865  RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
866  Q_EMIT rtabmapEventInitReceived((int)rtabmapEventInit->getStatus(), rtabmapEventInit->getInfo().c_str());
867  }
868  }
869  else if(anEvent->getClassName().compare("RtabmapEvent3DMap") == 0)
870  {
871  RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
872  Q_EMIT rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
873  }
874  else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
875  {
876  RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
877  Q_EMIT rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
878  }
879  else if(anEvent->getClassName().compare("RtabmapLabelErrorEvent") == 0)
880  {
881  RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
882  Q_EMIT rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
883  }
884  else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
885  {
886  Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode());
887  }
888  else if(anEvent->getClassName().compare("CameraEvent") == 0)
889  {
890  CameraEvent * cameraEvent = (CameraEvent*)anEvent;
891  if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
892  {
893  if(_preferencesDialog->beepOnPause())
894  {
895  QMetaObject::invokeMethod(this, "beep");
896  }
897  Q_EMIT noMoreImagesReceived();
898  }
899  else
900  {
901  Q_EMIT cameraInfoReceived(cameraEvent->info());
902  if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
903  {
904  OdometryInfo odomInfo;
905  odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
906  if (!_processingOdometry && !_processingStatistics)
907  {
908  _processingOdometry = true; // if we receive too many odometry events!
909  OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo);
910  Q_EMIT odometryReceived(tmp, false);
911  }
912  else
913  {
914  // we receive too many odometry events! ignore them
915  }
916  }
917  }
918  }
919  else if(anEvent->getClassName().compare("OdometryEvent") == 0)
920  {
921  OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
922  if(!_processingOdometry && !_processingStatistics)
923  {
924  _processingOdometry = true; // if we receive too many odometry events!
925  Q_EMIT odometryReceived(*odomEvent, false);
926  }
927  else
928  {
929  // we receive too many odometry events! just send without data
930  SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
931  data.setCameraModels(odomEvent->data().cameraModels());
932  data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
933  data.setGroundTruth(odomEvent->data().groundTruth());
934  OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
935  Q_EMIT odometryReceived(tmp, true);
936  }
937  }
938  else if(anEvent->getClassName().compare("ULogEvent") == 0)
939  {
940  ULogEvent * logEvent = (ULogEvent*)anEvent;
941  if(logEvent->getCode() >= _preferencesDialog->getGeneralLoggerPauseLevel())
942  {
943  QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
944  // The timer prevents multiple calls to pauseDetection() before the state can be changed
945  if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
946  {
947  _logEventTime->start();
948  if(_preferencesDialog->beepOnPause())
949  {
950  QMetaObject::invokeMethod(this, "beep");
951  }
952  pauseDetection();
953  }
954  }
955  }
956  return false;
957 }
958 
959 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
960 {
961  if(_firstStamp == 0.0)
962  {
963  _firstStamp = info.stamp;
964  }
965  _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures());
966  _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures());
967  _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
968  _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
969  _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
970  _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
971  _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
972  _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
973  _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
974 
975  Q_EMIT(cameraInfoProcessed());
976 }
977 
978 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
979 {
980  if(_firstStamp == 0.0)
981  {
982  _firstStamp = odom.data().stamp();
983  }
984 
985  UDEBUG("");
986  _processingOdometry = true;
987  UTimer time;
988  // Process Data
989 
990  // Set color code as tooltip
991  if(_ui->imageView_odometry->toolTip().isEmpty())
992  {
993  _ui->imageView_odometry->setToolTip(
994  "Background Color Code:\n"
995  " Dark Red = Odometry Lost\n"
996  " Dark Yellow = Low Inliers\n"
997  "Feature Color code:\n"
998  " Green = Inliers\n"
999  " Yellow = Not matched features from previous frame(s)\n"
1000  " Red = Outliers");
1001  }
1002 
1003  Transform pose = odom.pose();
1004  bool lost = false;
1005  bool lostStateChanged = false;
1006 
1007  if(pose.isNull())
1008  {
1009  UDEBUG("odom lost"); // use last pose
1010  lostStateChanged = _cloudViewer->getBackgroundColor() != Qt::darkRed;
1011  _cloudViewer->setBackgroundColor(Qt::darkRed);
1012  _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
1013 
1014  pose = _lastOdomPose;
1015  lost = true;
1016  }
1017  else if(odom.info().reg.inliers>0 &&
1018  _preferencesDialog->getOdomQualityWarnThr() &&
1019  odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
1020  {
1021  UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
1022  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
1023  _cloudViewer->setBackgroundColor(Qt::darkYellow);
1024  _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
1025  }
1026  else
1027  {
1028  UDEBUG("odom ok");
1029  lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
1030  _cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
1031  _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
1032  }
1033 
1034  if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
1035  {
1036  _lastOdomPose = pose;
1037  }
1038 
1039  if(_ui->dockWidget_cloudViewer->isVisible())
1040  {
1041  bool cloudUpdated = false;
1042  bool scanUpdated = false;
1043  bool featuresUpdated = false;
1044  bool filteredGravityUpdated = false;
1045  bool accelerationUpdated = false;
1046  if(!pose.isNull())
1047  {
1048  // 3d cloud
1049  if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
1050  odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
1051  !odom.data().depthOrRightRaw().empty() &&
1052  (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
1053  _preferencesDialog->isCloudsShown(1))
1054  {
1055  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1056  pcl::IndicesPtr indices(new std::vector<int>);
1057  cloud = util3d::cloudRGBFromSensorData(odom.data(),
1058  _preferencesDialog->getCloudDecimation(1),
1059  _preferencesDialog->getCloudMaxDepth(1),
1060  _preferencesDialog->getCloudMinDepth(1),
1061  indices.get(),
1062  _preferencesDialog->getAllParameters(),
1063  _preferencesDialog->getCloudRoiRatios(1));
1064  if(indices->size())
1065  {
1066  cloud = util3d::transformPointCloud(cloud, pose);
1067 
1068  if(_preferencesDialog->isCloudMeshing())
1069  {
1070  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
1071  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
1072  output = util3d::extractIndices(cloud, indices, false, true);
1073 
1074  // Fast organized mesh
1075  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
1076  if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
1077  {
1078  viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
1079  viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
1080  viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
1081  }
1082  else if(!odom.data().stereoCameraModel().localTransform().isNull())
1083  {
1084  viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
1085  viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
1086  viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
1087  }
1088  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
1089  output,
1090  _preferencesDialog->getCloudMeshingAngle(),
1091  _preferencesDialog->isCloudMeshingQuad(),
1092  _preferencesDialog->getCloudMeshingTriangleSize(),
1093  Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
1094  if(polygons.size())
1095  {
1096  if(_preferencesDialog->isCloudMeshingTexture() && !odom.data().imageRaw().empty())
1097  {
1098  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
1099  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1100  textureMesh->tex_polygons.push_back(polygons);
1101  int w = cloud->width;
1102  int h = cloud->height;
1103  UASSERT(w > 1 && h > 1);
1104  textureMesh->tex_coordinates.resize(1);
1105  int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
1106  textureMesh->tex_coordinates[0].resize(nPoints);
1107  for(int i=0; i<nPoints; ++i)
1108  {
1109  //uv
1110  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
1111  float(i % w) / float(w), // u
1112  float(h - i / w) / float(h)); // v
1113  }
1114 
1115  pcl::TexMaterial mesh_material;
1116  mesh_material.tex_d = 1.0f;
1117  mesh_material.tex_Ns = 75.0f;
1118  mesh_material.tex_illum = 1;
1119 
1120  mesh_material.tex_name = "material_odom";
1121  mesh_material.tex_file = "";
1122  textureMesh->tex_materials.push_back(mesh_material);
1123 
1124  if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, odom.data().imageRaw(), _odometryCorrection))
1125  {
1126  UERROR("Adding cloudOdom to viewer failed!");
1127  }
1128  }
1129  else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
1130  {
1131  UERROR("Adding cloudOdom to viewer failed!");
1132  }
1133  }
1134  }
1135  else
1136  {
1137  if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
1138  {
1139  UERROR("Adding cloudOdom to viewer failed!");
1140  }
1141  }
1142  _cloudViewer->setCloudVisibility("cloudOdom", true);
1143  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
1144  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
1145  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
1146 
1147  cloudUpdated = true;
1148  }
1149  }
1150 
1151  if(_preferencesDialog->isScansShown(1))
1152  {
1153  // F2M: scan local map
1154  if(!odom.info().localScanMap.isEmpty())
1155  {
1156  if(!lost)
1157  {
1158  bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanMapOdom");
1159  bool scanAdded = false;
1160  if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
1161  {
1162  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1163  util3d::laserScanToPointCloudINormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1164  _odometryCorrection, Qt::blue);
1165  }
1166  else if(odom.info().localScanMap.hasNormals())
1167  {
1168  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1169  util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1170  _odometryCorrection, Qt::blue);
1171  }
1172  else if(odom.info().localScanMap.hasIntensity())
1173  {
1174  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1175  util3d::laserScanToPointCloudI(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1176  _odometryCorrection, Qt::blue);
1177  }
1178  else
1179  {
1180  scanAdded = _cloudViewer->addCloud("scanMapOdom",
1181  util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
1182  _odometryCorrection, Qt::blue);
1183  }
1184 
1185 
1186  if(!scanAdded)
1187  {
1188  UERROR("Adding scanMapOdom to viewer failed!");
1189  }
1190  else
1191  {
1192  _cloudViewer->setCloudVisibility("scanMapOdom", true);
1193  _cloudViewer->setCloudColorIndex("scanMapOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1194  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
1195  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
1196  }
1197  }
1198  scanUpdated = true;
1199  }
1200  // scan cloud
1201  if(!odom.data().laserScanRaw().isEmpty())
1202  {
1203  LaserScan scan = odom.data().laserScanRaw();
1204 
1205  if(_preferencesDialog->getDownsamplingStepScan(1) > 1 ||
1206  _preferencesDialog->getScanMaxRange(1) > 0.0f ||
1207  _preferencesDialog->getScanMinRange(1) > 0.0f)
1208  {
1209  scan = util3d::commonFiltering(scan,
1210  _preferencesDialog->getDownsamplingStepScan(1),
1211  _preferencesDialog->getScanMinRange(1),
1212  _preferencesDialog->getScanMaxRange(1));
1213  }
1214  bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanOdom");
1215  bool scanAdded = false;
1216 
1217  if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
1218  {
1219  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud;
1220  cloud = util3d::laserScanToPointCloudINormal(scan, pose*scan.localTransform());
1221  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1222  {
1223  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1224  }
1225  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1226  }
1227  else if(odom.info().localScanMap.hasNormals())
1228  {
1229  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
1230  cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
1231  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1232  {
1233  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1234  }
1235  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1236  }
1237  else if(odom.info().localScanMap.hasIntensity())
1238  {
1239  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
1240  cloud = util3d::laserScanToPointCloudI(scan, pose*scan.localTransform());
1241  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1242  {
1243  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1244  }
1245  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1246  }
1247  else
1248  {
1249  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
1250  cloud = util3d::laserScanToPointCloud(scan, pose*scan.localTransform());
1251  if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
1252  {
1253  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
1254  }
1255  scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
1256  }
1257 
1258  if(!scanAdded)
1259  {
1260  UERROR("Adding scanOdom to viewer failed!");
1261  }
1262  else
1263  {
1264  _cloudViewer->setCloudVisibility("scanOdom", true);
1265  _cloudViewer->setCloudColorIndex("scanOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
1266  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
1267  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
1268  scanUpdated = true;
1269  }
1270  }
1271  }
1272 
1273  // 3d features
1274  if(_preferencesDialog->isFeaturesShown(1) && !odom.info().localMap.empty())
1275  {
1276  if(!lost)
1277  {
1278  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1279  cloud->resize(odom.info().localMap.size());
1280  int i=0;
1281  for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
1282  {
1283  // filter very far features from current location
1284  if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 100*100)
1285  {
1286  (*cloud)[i].x = iter->second.x;
1287  (*cloud)[i].y = iter->second.y;
1288  (*cloud)[i].z = iter->second.z;
1289 
1290  // green = inlier, yellow = outliers
1291  bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
1292  (*cloud)[i].r = inlier?0:255;
1293  (*cloud)[i].g = 255;
1294  (*cloud)[i].b = 0;
1295  if(!_preferencesDialog->isOdomOnlyInliersShown() || inlier)
1296  {
1297  ++i;
1298  }
1299  }
1300  }
1301  cloud->resize(i);
1302 
1303  _cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
1304  _cloudViewer->setCloudVisibility("featuresOdom", true);
1305  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
1306  }
1307  featuresUpdated = true;
1308  }
1309 
1310  if(_preferencesDialog->isFrustumsShown(1))
1311  {
1312  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
1313  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
1314  {
1315  std::list<std::string> splitted = uSplitNumChar(iter.key());
1316  if(splitted.size() == 2)
1317  {
1318  int id = std::atoi(splitted.back().c_str());
1319  if(splitted.front().compare("f_odom_") == 0 &&
1320  odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
1321  {
1322  _cloudViewer->removeFrustum(iter.key());
1323  }
1324  }
1325  }
1326 
1327  for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
1328  {
1329  std::string frustumId = uFormat("f_odom_%d", iter->first);
1330  if(_cloudViewer->getAddedFrustums().contains(frustumId))
1331  {
1332  _cloudViewer->updateFrustumPose(frustumId, _odometryCorrection*iter->second);
1333  }
1334  else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
1335  {
1336  const CameraModel & model = odom.info().localBundleModels.at(iter->first);
1337  Transform t = model.localTransform();
1338  if(!t.isNull())
1339  {
1340  QColor color = Qt::yellow;
1341  _cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
1342  }
1343  }
1344  }
1345 
1346  if( _preferencesDialog->isIMUGravityShown(1) &&
1347  (odom.data().imu().orientation().val[0]!=0 ||
1348  odom.data().imu().orientation().val[1]!=0 ||
1349  odom.data().imu().orientation().val[2]!=0 ||
1350  odom.data().imu().orientation().val[3]!=0))
1351  {
1352  Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
1353  Transform orientation(0,0,0, odom.data().imu().orientation()[0], odom.data().imu().orientation()[1], odom.data().imu().orientation()[2], odom.data().imu().orientation()[3]);
1354  gravity = (orientation* odom.data().imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
1355  _cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
1356  filteredGravityUpdated = true;
1357  }
1358  if( _preferencesDialog->isIMUAccShown() &&
1359  (odom.data().imu().linearAcceleration().val[0]!=0 ||
1360  odom.data().imu().linearAcceleration().val[1]!=0 ||
1361  odom.data().imu().linearAcceleration().val[2]!=0))
1362  {
1363  Eigen::Vector3f gravity(
1364  -odom.data().imu().linearAcceleration().val[0],
1365  -odom.data().imu().linearAcceleration().val[1],
1366  -odom.data().imu().linearAcceleration().val[2]);
1367  gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
1368  gravity = odom.data().imu().localTransform().toEigen3f()*gravity;
1369  _cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
1370  accelerationUpdated = true;
1371  }
1372  }
1373  }
1374  if(!dataIgnored)
1375  {
1376  if(!cloudUpdated && _cloudViewer->getAddedClouds().contains("cloudOdom"))
1377  {
1378  _cloudViewer->setCloudVisibility("cloudOdom", false);
1379  }
1380  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanOdom"))
1381  {
1382  _cloudViewer->setCloudVisibility("scanOdom", false);
1383  }
1384  if(!scanUpdated && _cloudViewer->getAddedClouds().contains("scanMapOdom"))
1385  {
1386  _cloudViewer->setCloudVisibility("scanMapOdom", false);
1387  }
1388  if(!featuresUpdated && _cloudViewer->getAddedClouds().contains("featuresOdom"))
1389  {
1390  _cloudViewer->setCloudVisibility("featuresOdom", false);
1391  }
1392  if(!filteredGravityUpdated && _cloudViewer->getAddedLines().find("odom_imu_orientation") != _cloudViewer->getAddedLines().end())
1393  {
1394  _cloudViewer->removeLine("odom_imu_orientation");
1395  }
1396  if(!accelerationUpdated && _cloudViewer->getAddedLines().find("odom_imu_acc") != _cloudViewer->getAddedLines().end())
1397  {
1398  _cloudViewer->removeLine("odom_imu_acc");
1399  }
1400  }
1401  }
1402 
1403  if(!odom.pose().isNull())
1404  {
1405  _odometryReceived = true;
1406  // update camera position
1407  if(odom.data().cameraModels().size() && odom.data().cameraModels()[0].isValidForProjection())
1408  {
1409  _cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
1410  }
1411  else if(odom.data().stereoCameraModel().isValidForProjection())
1412  {
1413  _cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
1414  }
1415 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
1416  if(_preferencesDialog->isFramesShown())
1417  {
1418  _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), false, false);
1419  }
1420  else
1421  {
1422  _cloudViewer->removeLine("odom_to_base_link");
1423  }
1424 #endif
1425  _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
1426  }
1427  _cloudViewer->update();
1428 
1429  if(_ui->graphicsView_graphView->isVisible())
1430  {
1431  if(!pose.isNull() && !odom.pose().isNull())
1432  {
1433  _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
1434  _ui->graphicsView_graphView->update();
1435  }
1436  }
1437 
1438  if(_ui->dockWidget_odometry->isVisible() &&
1439  !odom.data().imageRaw().empty())
1440  {
1441  if(_ui->imageView_odometry->isFeaturesShown())
1442  {
1443  if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2)
1444  {
1445  if(_preferencesDialog->isOdomOnlyInliersShown())
1446  {
1447  std::multimap<int, cv::KeyPoint> kpInliers;
1448  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1449  {
1450  kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
1451  }
1452  _ui->imageView_odometry->setFeatures(
1453  kpInliers,
1454  odom.data().depthRaw(),
1455  Qt::green);
1456  }
1457  else
1458  {
1459  _ui->imageView_odometry->setFeatures(
1460  odom.info().words,
1461  odom.data().depthRaw(),
1462  Qt::yellow);
1463  }
1464  }
1465  else if(odom.info().type == (int)Odometry::kTypeF2F ||
1466  odom.info().type == (int)Odometry::kTypeViso2 ||
1467  odom.info().type == (int)Odometry::kTypeFovis ||
1468  odom.info().type == (int)Odometry::kTypeMSCKF ||
1469  odom.info().type == (int)Odometry::kTypeVINS)
1470  {
1471  std::vector<cv::KeyPoint> kpts;
1472  cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
1473  _ui->imageView_odometry->setFeatures(
1474  kpts,
1475  odom.data().depthRaw(),
1476  Qt::red);
1477  }
1478  }
1479 
1480  //detect if it is OdometryMono initialization
1481  bool monoInitialization = false;
1482  if(_preferencesDialog->getOdomStrategy() == 6 && odom.info().type == (int)Odometry::kTypeF2F)
1483  {
1484  monoInitialization = true;
1485  }
1486 
1487  _ui->imageView_odometry->clearLines();
1488  if(lost && !monoInitialization)
1489  {
1490  if(lostStateChanged)
1491  {
1492  // save state
1493  _odomImageShow = _ui->imageView_odometry->isImageShown();
1494  _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
1495  }
1496  _ui->imageView_odometry->setImageDepth(odom.data().imageRaw());
1497  _ui->imageView_odometry->setImageShown(true);
1498  _ui->imageView_odometry->setImageDepthShown(true);
1499  }
1500  else
1501  {
1502  if(lostStateChanged)
1503  {
1504  // restore state
1505  _ui->imageView_odometry->setImageShown(_odomImageShow);
1506  _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
1507  }
1508 
1509  _ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
1510  if(_ui->imageView_odometry->isImageDepthShown() && !odom.data().depthOrRightRaw().empty())
1511  {
1512  _ui->imageView_odometry->setImageDepth(odom.data().depthOrRightRaw());
1513  }
1514 
1515  if( odom.info().type == (int)Odometry::kTypeF2M ||
1516  odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
1517  odom.info().type == (int)Odometry::kTypeMSCKF ||
1518  odom.info().type == (int)Odometry::kTypeVINS)
1519  {
1520  if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
1521  {
1522  for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
1523  {
1524  _ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
1525  }
1526  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
1527  {
1528  _ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
1529  }
1530  }
1531  }
1532  if((odom.info().type == (int)Odometry::kTypeF2F ||
1533  odom.info().type == (int)Odometry::kTypeViso2 ||
1534  odom.info().type == (int)Odometry::kTypeFovis) && odom.info().refCorners.size())
1535  {
1536  if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
1537  {
1538  //draw lines
1539  UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
1540  std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
1541  for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
1542  {
1543  if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
1544  {
1545  _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
1546  }
1547  if(_ui->imageView_odometry->isLinesShown())
1548  {
1549  _ui->imageView_odometry->addLine(
1550  odom.info().newCorners[i].x,
1551  odom.info().newCorners[i].y,
1552  odom.info().refCorners[i].x,
1553  odom.info().refCorners[i].y,
1554  inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
1555  }
1556  }
1557  }
1558  }
1559  }
1560  if(!odom.data().imageRaw().empty())
1561  {
1562  _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
1563  }
1564 
1565  _ui->imageView_odometry->update();
1566  }
1567 
1568  if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
1569  {
1570  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
1571  }
1572 
1573  //Process info
1574  _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
1575  _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
1576  _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
1577  _ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
1578  _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
1579  _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
1580  _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
1581  _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
1582  _ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
1583  _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
1584  _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
1585  _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
1586  _ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
1587  _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
1588  _ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
1589  _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
1590  _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1591  if(odom.info().timeParticleFiltering>0.0f)
1592  {
1593  _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1594  }
1595  if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
1596  {
1597  _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
1598  _ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
1599  }
1600  _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
1601  _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
1602  _ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
1603  _ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
1604  if(odom.info().localBundleTime > 0.0f)
1605  {
1606  _ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
1607  _ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
1608  _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1609  }
1610  _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
1611  _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
1612 
1613  Transform odomT;
1614  float dist=0.0f, x,y,z, roll,pitch,yaw;
1615  if(!odom.info().transform.isNull())
1616  {
1617  odomT = odom.info().transform;
1618  odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1619  dist = odom.info().transform.getNorm();
1620  _ui->statsToolBox->updateStat("Odometry/T/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
1621  _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1622  _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1623  _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1624  _ui->statsToolBox->updateStat("Odometry/Troll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1625  _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1626  _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1627  }
1628 
1629  if(!odom.info().transformFiltered.isNull())
1630  {
1631  odomT = odom.info().transformFiltered;
1632  odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1633  dist = odom.info().transformFiltered.getNorm();
1634  _ui->statsToolBox->updateStat("Odometry/TF/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
1635  _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1636  _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1637  _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1638  _ui->statsToolBox->updateStat("Odometry/TFroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1639  _ui->statsToolBox->updateStat("Odometry/TFpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1640  _ui->statsToolBox->updateStat("Odometry/TFyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1641  }
1642  if(odom.info().interval > 0)
1643  {
1644  _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures());
1645  _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1646  _ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
1647  _ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
1648  }
1649 
1650  if(!odom.info().guessVelocity.isNull())
1651  {
1652  odom.info().guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1653  dist = odom.info().guessVelocity.getNorm();
1654  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*3.6f, _preferencesDialog->isCacheSavedInFigures());
1655  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*2.237f, _preferencesDialog->isCacheSavedInFigures());
1656  _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
1657  }
1658 
1659  if(!odom.info().transformGroundTruth.isNull())
1660  {
1661  if(!odomT.isNull())
1662  {
1663  rtabmap::Transform diff = odom.info().transformGroundTruth.inverse()*odomT;
1664  _ui->statsToolBox->updateStat("Odometry/TG_error_lin/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), diff.getNorm(), _preferencesDialog->isCacheSavedInFigures());
1665  _ui->statsToolBox->updateStat("Odometry/TG_error_ang/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), diff.getAngle()*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1666  }
1667 
1668  odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1669  dist = odom.info().transformGroundTruth.getNorm();
1670  _ui->statsToolBox->updateStat("Odometry/TG/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
1671  _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1672  _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1673  _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1674  _ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1675  _ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1676  _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1677  if(odom.info().interval > 0)
1678  {
1679  _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
1680  _ui->statsToolBox->updateStat("Odometry/SpeedG/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
1681  _ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
1682  }
1683  }
1684 
1685  //cumulative pose
1686  if(!odom.pose().isNull())
1687  {
1688  odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1689  _ui->statsToolBox->updateStat("Odometry/Px/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1690  _ui->statsToolBox->updateStat("Odometry/Py/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1691  _ui->statsToolBox->updateStat("Odometry/Pz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1692  _ui->statsToolBox->updateStat("Odometry/Proll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1693  _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1694  _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1695  }
1696  if(!odom.data().groundTruth().isNull())
1697  {
1698  odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1699  _ui->statsToolBox->updateStat("Odometry/PGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures());
1700  _ui->statsToolBox->updateStat("Odometry/PGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures());
1701  _ui->statsToolBox->updateStat("Odometry/PGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures());
1702  _ui->statsToolBox->updateStat("Odometry/PGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1703  _ui->statsToolBox->updateStat("Odometry/PGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1704  _ui->statsToolBox->updateStat("Odometry/PGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
1705  }
1706 
1707  if(odom.info().distanceTravelled > 0)
1708  {
1709  _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures());
1710  }
1711 
1712  _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures());
1713  _processingOdometry = false;
1714 
1715  Q_EMIT(odometryProcessed());
1716 }
1717 
1718 void MainWindow::processStats(const rtabmap::Statistics & stat)
1719 {
1720  _processingStatistics = true;
1721  ULOGGER_DEBUG("");
1722  QTime time, totalTime;
1723  time.start();
1724  totalTime.start();
1725  //Affichage des stats et images
1726 
1727  if(_firstStamp == 0.0)
1728  {
1729  _firstStamp = stat.stamp();
1730  }
1731 
1732  int refMapId = -1, loopMapId = -1;
1733  if(stat.getLastSignatureData().id() == stat.refImageId())
1734  {
1735  refMapId = stat.getLastSignatureData().mapId();
1736  }
1737  int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
1738  int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
1739  if(loopId>0 && _cachedSignatures.contains(loopId))
1740  {
1741  loopMapId = _cachedSignatures.value(loopId).mapId();
1742  }
1743 
1744  _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
1745 
1746  if(stat.extended())
1747  {
1748  float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
1749  if(totalTime/1000.0f > float(1.0/_preferencesDialog->getDetectionRate()))
1750  {
1751  UWARN("Processing time (%fs) is over detection rate (%fs), real-time problem!", totalTime/1000.0f, 1.0/_preferencesDialog->getDetectionRate());
1752  }
1753 
1754  UDEBUG("");
1755  bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
1756 
1757  bool smallMovement = (bool)uValue(stat.data(), Statistics::kMemorySmall_movement(), 0.0f);
1758 
1759  bool fastMovement = (bool)uValue(stat.data(), Statistics::kMemoryFast_movement(), 0.0f);
1760 
1761  // update cache
1762  Signature signature;
1763  if(stat.getLastSignatureData().id() == stat.refImageId())
1764  {
1765  signature = stat.getLastSignatureData();
1766 
1767  // make sure data are uncompressed
1768  // We don't need to uncompress images if we don't show them
1769  bool uncompressImages = !signature.sensorData().imageCompressed().empty() && (
1770  _ui->imageView_source->isVisible() ||
1771  (_loopClosureViewer->isVisible() &&
1772  !signature.sensorData().depthOrRightCompressed().empty()) ||
1773  (_cloudViewer->isVisible() &&
1774  _preferencesDialog->isCloudsShown(0) &&
1775  !signature.sensorData().depthOrRightCompressed().empty()));
1776  bool uncompressScan = !signature.sensorData().laserScanCompressed().isEmpty() && (
1777  _loopClosureViewer->isVisible() ||
1778  (_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0)));
1779  cv::Mat tmpRgb, tmpDepth, tmpG, tmpO, tmpE;
1780  LaserScan tmpScan;
1781  signature.sensorData().uncompressData(
1782  uncompressImages?&tmpRgb:0,
1783  uncompressImages?&tmpDepth:0,
1784  uncompressScan?&tmpScan:0,
1785  0, &tmpG, &tmpO, &tmpE);
1786 
1787  if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
1788  signature.getWeight()>=0) // ignore intermediate nodes for the cache
1789  {
1790  if(smallMovement || fastMovement)
1791  {
1792  _cachedSignatures.insert(0, signature); // zero means temporary
1793  }
1794  else
1795  {
1796  _cachedSignatures.insert(signature.id(), signature);
1797  _cachedMemoryUsage += signature.sensorData().getMemoryUsed();
1798  unsigned int count = 0;
1799  if(!signature.getWords3().empty())
1800  {
1801  for(std::multimap<int, int>::const_iterator jter=signature.getWords().upper_bound(-1); jter!=signature.getWords().end(); ++jter)
1802  {
1803  if(util3d::isFinite(signature.getWords3()[jter->second]))
1804  {
1805  ++count;
1806  }
1807  }
1808  }
1809  _cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
1810  }
1811  }
1812  }
1813 
1814  // For intermediate empty nodes, keep latest image shown
1815  if(signature.getWeight() >= 0 &&
1816  (!signature.sensorData().imageRaw().empty() || signature.getWords().size()))
1817  {
1818  _ui->imageView_source->clear();
1819  _ui->imageView_loopClosure->clear();
1820 
1821  _ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
1822  _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
1823 
1824  _ui->label_matchId->clear();
1825 
1826 
1827  int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1828  bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
1829  int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
1830  bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
1831  _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
1832 
1833  if(rehearsalMerged > 0)
1834  {
1835  _ui->imageView_source->setBackgroundColor(Qt::blue);
1836  }
1837  else if(proximityTimeDetections > 0)
1838  {
1839  _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
1840  }
1841  else if(scanMatchingSuccess)
1842  {
1843  _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
1844  }
1845  else if(rehearsedSimilarity)
1846  {
1847  _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
1848  }
1849  else if(smallMovement)
1850  {
1851  _ui->imageView_source->setBackgroundColor(Qt::gray);
1852  }
1853  else if(fastMovement)
1854  {
1855  _ui->imageView_source->setBackgroundColor(Qt::magenta);
1856  }
1857  // Set color code as tooltip
1858  if(_ui->label_refId->toolTip().isEmpty())
1859  {
1860  _ui->label_refId->setToolTip(
1861  "Background Color Code:\n"
1862  " Blue = Weight Update Merged\n"
1863  " Dark Blue = Weight Update\n"
1864  " Dark Yellow = Proximity Detection in Time\n"
1865  " Dark Cyan = Neighbor Link Refined\n"
1866  " Gray = Small Movement\n"
1867  " Magenta = Fast Movement\n"
1868  "Feature Color code:\n"
1869  " Green = New\n"
1870  " Yellow = New but Not Unique\n"
1871  " Red = In Vocabulary\n"
1872  " Blue = In Vocabulary and in Previous Signature\n"
1873  " Pink = In Vocabulary and in Loop Closure Signature\n"
1874  " Gray = Not Quantized to Vocabulary");
1875  }
1876  // Set color code as tooltip
1877  if(_ui->label_matchId->toolTip().isEmpty())
1878  {
1879  _ui->label_matchId->setToolTip(
1880  "Background Color Code:\n"
1881  " Green = Accepted Loop Closure Detection\n"
1882  " Red = Rejected Loop Closure Detection\n"
1883  " Yellow = Proximity Detection in Space\n"
1884  "Feature Color code:\n"
1885  " Red = In Vocabulary\n"
1886  " Pink = In Vocabulary and in Loop Closure Signature\n"
1887  " Gray = Not Quantized to Vocabulary");
1888  }
1889 
1890  UDEBUG("time= %d ms", time.restart());
1891 
1892  int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
1893  float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
1894  int landmarkId = static_cast<int>(uValue(stat.data(), Statistics::kLoopLandmark_detected(), 0.0f));
1895  int landmarkNodeRef = static_cast<int>(uValue(stat.data(), Statistics::kLoopLandmark_detected_node_ref(), 0.0f));
1896  int matchId = 0;
1897  Signature loopSignature;
1898  int shownLoopId = 0;
1899  if(highestHypothesisId > 0 || stat.proximityDetectionId()>0 || landmarkId>0)
1900  {
1901  bool show = true;
1902  if(stat.loopClosureId() > 0)
1903  {
1904  _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
1905  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
1906  if(highestHypothesisIsSaved)
1907  {
1908  _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
1909  }
1910  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
1911  matchId = stat.loopClosureId();
1912  }
1913  else if(stat.proximityDetectionId())
1914  {
1915  _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
1916  _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
1917  matchId = stat.proximityDetectionId();
1918  }
1919  else if(landmarkId!=0)
1920  {
1921  _ui->imageView_loopClosure->setBackgroundColor(QColor("orange"));
1922  _ui->label_matchId->setText(QString("Landmark match = %1 with %2").arg(landmarkId).arg(landmarkNodeRef));
1923  matchId = landmarkNodeRef;
1924  }
1925  else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
1926  {
1927  show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
1928  if(show)
1929  {
1930  _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
1931  _ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
1932  _ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
1933  }
1934  }
1935  else
1936  {
1937  show = _preferencesDialog->imageHighestHypShown();
1938  if(show)
1939  {
1940  _ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
1941  }
1942  }
1943 
1944  if(show)
1945  {
1946  shownLoopId = matchId>0?matchId:highestHypothesisId;
1947  QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
1948  if(iter != _cachedSignatures.end())
1949  {
1950  // uncompress after copy to avoid keeping uncompressed data in memory
1951  loopSignature = iter.value();
1952  bool uncompressImages = _ui->imageView_source->isVisible() ||
1953  (_loopClosureViewer->isVisible() && !signature.sensorData().depthOrRightCompressed().empty());
1954  bool uncompressScan = _loopClosureViewer->isVisible() && !signature.sensorData().laserScanCompressed().isEmpty();
1955  if(uncompressImages || uncompressScan)
1956  {
1957  cv::Mat tmpRGB, tmpDepth;
1958  LaserScan tmpScan;
1959  loopSignature.sensorData().uncompressData(
1960  uncompressImages?&tmpRGB:0,
1961  uncompressImages?&tmpDepth:0,
1962  uncompressScan?&tmpScan:0);
1963  }
1964  }
1965  }
1966  }
1967  _refIds.push_back(stat.refImageId());
1968  _loopClosureIds.push_back(matchId);
1969  if(matchId > 0)
1970  {
1971  _cachedLocalizationsCount[matchId] += 1.0f;
1972  }
1973 
1974  //update image views
1975  {
1976  cv::Mat refImage = signature.sensorData().imageRaw();
1977  cv::Mat loopImage = loopSignature.sensorData().imageRaw();
1978 
1979  if( _preferencesDialog->isMarkerDetection() &&
1980  _preferencesDialog->isLandmarksShown())
1981  {
1982  //draw markers
1983  if(!signature.getLandmarks().empty())
1984  {
1985  refImage = refImage.clone();
1986  drawLandmarks(refImage, signature);
1987  }
1988  if(!loopSignature.getLandmarks().empty())
1989  {
1990  loopImage = loopImage.clone();
1991  drawLandmarks(loopImage, loopSignature);
1992  }
1993  }
1994 
1995  UCvMat2QImageThread qimageThread(refImage);
1996  UCvMat2QImageThread qimageLoopThread(loopImage);
1997  qimageThread.start();
1998  qimageLoopThread.start();
1999  qimageThread.join();
2000  qimageLoopThread.join();
2001  QImage img = qimageThread.getQImage();
2002  QImage lcImg = qimageLoopThread.getQImage();
2003  UDEBUG("time= %d ms", time.restart());
2004 
2005  if(!img.isNull())
2006  {
2007  _ui->imageView_source->setImage(img);
2008  }
2009  if(!signature.sensorData().depthOrRightRaw().empty())
2010  {
2011  _ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
2012  }
2013  if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
2014  {
2015  QRect sceneRect;
2016  if(signature.sensorData().cameraModels().size())
2017  {
2018  for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
2019  {
2020  sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
2021  sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
2022  }
2023  }
2024  else if(signature.sensorData().stereoCameraModel().isValidForProjection())
2025  {
2026  sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
2027  }
2028  if(sceneRect.isValid())
2029  {
2030  _ui->imageView_source->setSceneRect(sceneRect);
2031  }
2032  }
2033  if(!lcImg.isNull())
2034  {
2035  _ui->imageView_loopClosure->setImage(lcImg);
2036  }
2037  if(!loopSignature.sensorData().depthOrRightRaw().empty())
2038  {
2039  _ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
2040  }
2041  if(_ui->imageView_loopClosure->sceneRect().isNull())
2042  {
2043  _ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
2044  }
2045  }
2046 
2047  UDEBUG("time= %d ms", time.restart());
2048 
2049  // do it after scaling
2050  std::multimap<int, cv::KeyPoint> wordsA;
2051  std::multimap<int, cv::KeyPoint> wordsB;
2052  for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
2053  {
2054  wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
2055  }
2056  for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
2057  {
2058  wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
2059  }
2060  this->drawKeypoints(wordsA, wordsB);
2061 
2062  UDEBUG("time= %d ms", time.restart());
2063 
2064  // loop closure view
2065  if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
2066  !stat.loopClosureTransform().isNull() &&
2067  !loopSignature.sensorData().imageRaw().empty())
2068  {
2069  // the last loop closure data
2070  Transform loopClosureTransform = stat.loopClosureTransform();
2071  signature.setPose(loopClosureTransform);
2072  _loopClosureViewer->setData(loopSignature, signature);
2073  if(_ui->dockWidget_loopClosureViewer->isVisible())
2074  {
2075  UTimer loopTimer;
2076  _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
2077  UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
2078  _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
2079  }
2080 
2081  UDEBUG("time= %d ms", time.restart());
2082  }
2083  }
2084 
2085  // PDF AND LIKELIHOOD
2086  if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
2087  {
2088  UDEBUG("");
2089  _posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
2090 
2091  ULOGGER_DEBUG("");
2092  //Adjust thresholds
2093  Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
2094  }
2095  if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
2096  {
2097  _likelihoodCurve->setData(QMap<int, float>(stat.likelihood()), QMap<int, int>(stat.weights()));
2098  }
2099  if(!stat.rawLikelihood().empty() && _ui->dockWidget_rawlikelihood->isVisible())
2100  {
2101  _rawLikelihoodCurve->setData(QMap<int, float>(stat.rawLikelihood()), QMap<int, int>(stat.weights()));
2102  }
2103 
2104  // Update statistics tool box
2105  const std::map<std::string, float> & statistics = stat.data();
2106  std::string odomStr = "Odometry/";
2107  for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
2108  {
2109  //ULOGGER_DEBUG("Updating stat \"%s\"", (*iter).first.c_str());
2110  if((*iter).first.size()<odomStr.size() || (*iter).first.substr(0, odomStr.size()).compare(odomStr)!=0)
2111  {
2112  _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), (*iter).second, _preferencesDialog->isCacheSavedInFigures());
2113  }
2114  }
2115 
2116  UDEBUG("time= %d ms", time.restart());
2117 
2118  //======================
2119  // RGB-D Mapping stuff
2120  //======================
2121  // update clouds
2122  if(stat.poses().size())
2123  {
2124  // update pose only if odometry is not received
2125  std::map<int, int> mapIds = _currentMapIds;
2126  std::map<int, Transform> groundTruth = _currentGTPosesMap;
2127  std::map<int, std::string> labels = _currentLabels;
2128 
2129  mapIds.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().mapId()));
2130  if(!stat.getLastSignatureData().getGroundTruthPose().isNull() &&
2131  _cachedSignatures.contains(stat.getLastSignatureData().id()))
2132  {
2133  groundTruth.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getGroundTruthPose()));
2134  }
2135  for(std::map<int, std::string>::const_iterator iter=stat.labels().begin(); iter!=stat.labels().end(); ++iter)
2136  {
2137  uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified
2138  }
2139 
2140  if(_preferencesDialog->isPriorIgnored() &&
2141  _ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
2142  stat.getLastSignatureData().sensorData().gps().stamp()!=0.0 &&
2143  stat.poses().find(stat.getLastSignatureData().id())!=stat.poses().end())
2144  {
2145  float bearing = (float)((-(stat.getLastSignatureData().sensorData().gps().bearing()-90))*M_PI/180.0);
2146  float gpsRotationOffset = stat.poses().at(stat.getLastSignatureData().id()).theta()-bearing;
2147  _ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset);
2148  }
2149  else if(!_preferencesDialog->isPriorIgnored() &&
2150  _ui->graphicsView_graphView->getWorldMapRotation() != 0.0f)
2151  {
2152  _ui->graphicsView_graphView->setWorldMapRotation(0.0f);
2153  }
2154 
2155 
2156  std::map<int, Transform> poses = stat.poses();
2157 
2158  UDEBUG("time= %d ms", time.restart());
2159 
2160 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2161  if(_preferencesDialog->isFramesShown())
2162  {
2163  _cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
2164  _cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
2165  _cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
2166  }
2167  else
2168  {
2169  _cloudViewer->removeLine("map_to_odom");
2170  _cloudViewer->removeCoordinate("odom_frame");
2171  _cloudViewer->removeCoordinate("map_frame");
2172  }
2173 #endif
2174 
2175  if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
2176  {
2177  if(poses.rbegin()->first == stat.getLastSignatureData().id())
2178  {
2179  if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
2180  {
2181  _cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
2182  }
2183  else if(stat.getLastSignatureData().sensorData().stereoCameraModel().isValidForProjection())
2184  {
2185  _cloudViewer->updateCameraFrustum(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModel());
2186  }
2187  }
2188 
2189  _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
2190 
2191  if(_ui->graphicsView_graphView->isVisible())
2192  {
2193  _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
2194  }
2195  }
2196 
2197  if(_cachedSignatures.contains(0) && stat.refImageId()>0)
2198  {
2199  if(poses.find(stat.refImageId())!=poses.end())
2200  {
2201  poses.insert(std::make_pair(0, poses.at(stat.refImageId())));
2202  poses.erase(stat.refImageId());
2203  }
2204  if(groundTruth.find(stat.refImageId())!=groundTruth.end())
2205  {
2206  groundTruth.insert(std::make_pair(0, groundTruth.at(stat.refImageId())));
2207  groundTruth.erase(stat.refImageId());
2208  }
2209  }
2210 
2211  std::map<std::string, float> updateCloudSats;
2212  updateMapCloud(
2213  poses,
2214  stat.constraints(),
2215  mapIds,
2216  labels,
2217  groundTruth,
2218  false,
2219  &updateCloudSats);
2220 
2221  _odometryReceived = false;
2222 
2223  UDEBUG("time= %d ms", time.restart());
2224 
2225  for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
2226  {
2227  _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
2228  }
2229  }
2230  _odometryCorrection = stat.mapCorrection();
2231 
2232  if( _ui->graphicsView_graphView->isVisible())
2233  {
2234  // update posterior on the graph view
2235  if(_preferencesDialog->isPosteriorGraphView() &&
2236  stat.posterior().size())
2237  {
2238  _ui->graphicsView_graphView->updatePosterior(stat.posterior());
2239  }
2240  else if(_preferencesDialog->isRGBDMode())
2241  {
2242  if(_preferencesDialog->isWordsCountGraphView() &&
2243  _cachedWordsCount.size())
2244  {
2245  _ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
2246  }
2247  else if(_preferencesDialog->isLocalizationsCountGraphView() &&
2248  _cachedLocalizationsCount.size())
2249  {
2250  _ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
2251  }
2252  }
2253  // update local path on the graph view
2254  _ui->graphicsView_graphView->updateLocalPath(stat.localPath());
2255  if(stat.localPath().size() == 0)
2256  {
2257  // clear the global path if set (goal reached)
2258  _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
2259  }
2260  // update current goal id
2261  if(stat.currentGoalId() > 0)
2262  {
2263  _ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
2264  }
2265  }
2266  UDEBUG("");
2267 
2268  _cachedSignatures.remove(0); // remove tmp negative ids
2269 
2270  // keep only compressed data in cache
2271  if(_cachedSignatures.contains(stat.refImageId()))
2272  {
2273  Signature & s = *_cachedSignatures.find(stat.refImageId());
2274  _cachedMemoryUsage -= s.sensorData().getMemoryUsed();
2275  s.sensorData().clearRawData();
2276  s.sensorData().clearOccupancyGridRaw();
2277  _cachedMemoryUsage += s.sensorData().getMemoryUsed();
2278  }
2279 
2280  UDEBUG("");
2281  }
2282  else if(!stat.extended() && stat.loopClosureId()>0)
2283  {
2284  _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
2285  _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
2286  }
2287  else
2288  {
2289  _ui->label_matchId->clear();
2290  }
2291  float elapsedTime = static_cast<float>(totalTime.elapsed());
2292  UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
2293  _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), elapsedTime, _preferencesDialog->isCacheSavedInFigures());
2294  if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
2295  {
2296  this->captureScreen(_autoScreenCaptureRAM, _autoScreenCapturePNG);
2297  }
2298 
2299  if(!_preferencesDialog->isImagesKept())
2300  {
2301  _cachedSignatures.clear();
2302  _cachedMemoryUsage = 0;
2303  _cachedWordsCount.clear();
2304  }
2305  _ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2306  _ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2307 #ifdef RTABMAP_OCTOMAP
2308  _ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024), _preferencesDialog->isCacheSavedInFigures());
2309 #endif
2310  if(_state != kMonitoring && _state != kDetecting)
2311  {
2312  _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2313  _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2314  _ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
2315  }
2316 
2317  _processingStatistics = false;
2318 
2319  Q_EMIT(statsProcessed());
2320 }
2321 
2322 void MainWindow::updateMapCloud(
2323  const std::map<int, Transform> & posesIn,
2324  const std::multimap<int, Link> & constraints,
2325  const std::map<int, int> & mapIdsIn,
2326  const std::map<int, std::string> & labels,
2327  const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
2328  bool verboseProgress,
2329  std::map<std::string, float> * stats)
2330 {
2331  UTimer timer;
2332  std::map<int, Transform> nodePoses(posesIn.lower_bound(0), posesIn.end());
2333  UDEBUG("nodes=%d landmarks=%d constraints=%d mapIdsIn=%d labelsIn=%d",
2334  (int)nodePoses.size(), (int)(posesIn.size() - nodePoses.size()), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
2335  if(posesIn.size())
2336  {
2337  _currentPosesMap = posesIn;
2338  _currentPosesMap.erase(0); // don't keep 0 if it is there
2339  _currentLinksMap = constraints;
2340  _currentMapIds = mapIdsIn;
2341  _currentLabels = labels;
2342  _currentGTPosesMap = groundTruths;
2343  _currentGTPosesMap.erase(0);
2344  if(_state != kMonitoring && _state != kDetecting)
2345  {
2346  _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && nodePoses.size() >= 2 && _currentLinksMap.size() >= 1);
2347  _ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
2348  }
2349  _ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
2350  }
2351 
2352  // filter duplicated poses
2353  std::map<int, Transform> poses;
2354  std::map<int, int> mapIds;
2355  if(_preferencesDialog->isCloudFiltering() && nodePoses.size())
2356  {
2357  float radius = _preferencesDialog->getCloudFilteringRadius();
2358  float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
2359  bool hasZero = nodePoses.find(0) != nodePoses.end();
2360  if(hasZero)
2361  {
2362  std::map<int, Transform> posesInTmp = nodePoses;
2363  posesInTmp.erase(0);
2364  poses = rtabmap::graph::radiusPosesFiltering(posesInTmp, radius, angle);
2365  }
2366  else
2367  {
2368  poses = rtabmap::graph::radiusPosesFiltering(nodePoses, radius, angle);
2369  }
2370  for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
2371  {
2372  std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
2373  if(jter!=mapIdsIn.end())
2374  {
2375  mapIds.insert(*jter);
2376  }
2377  }
2378  //keep 0
2379  if(hasZero)
2380  {
2381  poses.insert(*nodePoses.find(0));
2382  }
2383 
2384  if(verboseProgress)
2385  {
2386  _progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(nodePoses.size()));
2387  QApplication::processEvents();
2388  }
2389  }
2390  else
2391  {
2392  poses = nodePoses;
2393  mapIds = mapIdsIn;
2394  }
2395 
2396  std::map<int, bool> posesMask;
2397  for(std::map<int, Transform>::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter)
2398  {
2399  posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
2400  }
2401  _ui->widget_mapVisibility->setMap(nodePoses, posesMask);
2402 
2403  if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
2404  {
2405  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2406  {
2407  std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
2408  if(gtIter!=groundTruths.end())
2409  {
2410  iter->second = gtIter->second;
2411  }
2412  else
2413  {
2414  UWARN("Not found ground truth pose for node %d", iter->first);
2415  }
2416  }
2417  }
2418  else if(_currentGTPosesMap.size() == 0)
2419  {
2420  _ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
2421  }
2422 
2423  int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
2424  if(maxNodes > 0 && poses.size()>1)
2425  {
2426  std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
2427  std::map<int, Transform> nearestPoses;
2428  nearestPoses.insert(*poses.rbegin());
2429  for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2430  {
2431  std::map<int, Transform>::iterator pter = poses.find(iter->first);
2432  if(pter != poses.end())
2433  {
2434  nearestPoses.insert(*pter);
2435  }
2436  }
2437  //add zero...
2438  if(poses.find(0) != poses.end())
2439  {
2440  nearestPoses.insert(*poses.find(0));
2441  }
2442  poses=nearestPoses;
2443  }
2444 
2445  // Map updated! regenerate the assembled cloud, last pose is the new one
2446  UDEBUG("Update map with %d locations", poses.size());
2447  QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
2448  std::set<std::string> viewerLines = _cloudViewer->getAddedLines();
2449  int i=1;
2450  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2451  {
2452  if(!iter->second.isNull())
2453  {
2454  std::string cloudName = uFormat("cloud%d", iter->first);
2455 
2456  if(iter->first == 0)
2457  {
2458  viewerClouds.remove(cloudName);
2459  _cloudViewer->removeCloud(cloudName);
2460  }
2461 
2462  // 3d point cloud
2463  bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
2464  if(update3dCloud)
2465  {
2466  // update cloud
2467  if(viewerClouds.contains(cloudName))
2468  {
2469  // Update only if the pose has changed
2470  Transform tCloud;
2471  _cloudViewer->getPose(cloudName, tCloud);
2472  if(tCloud.isNull() || iter->second != tCloud)
2473  {
2474  if(!_cloudViewer->updateCloudPose(cloudName, iter->second))
2475  {
2476  UERROR("Updating pose cloud %d failed!", iter->first);
2477  }
2478  }
2479  _cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
2480  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
2481  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
2482  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
2483  }
2484  else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
2485  _cachedClouds.find(iter->first) == _cachedClouds.end() &&
2486  _cachedSignatures.contains(iter->first))
2487  {
2488  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2489  if(_cloudViewer->getAddedClouds().contains(cloudName))
2490  {
2491  _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
2492  }
2493  }
2494  }
2495  else if(viewerClouds.contains(cloudName))
2496  {
2497  _cloudViewer->setCloudVisibility(cloudName.c_str(), false);
2498  }
2499 
2500  // 2d point cloud
2501  std::string scanName = uFormat("scan%d", iter->first);
2502  if(iter->first == 0)
2503  {
2504  viewerClouds.remove(scanName);
2505  _cloudViewer->removeCloud(scanName);
2506  }
2507  if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
2508  {
2509  if(viewerClouds.contains(scanName))
2510  {
2511  // Update only if the pose has changed
2512  Transform tScan;
2513  _cloudViewer->getPose(scanName, tScan);
2514  if(tScan.isNull() || iter->second != tScan)
2515  {
2516  if(!_cloudViewer->updateCloudPose(scanName, iter->second))
2517  {
2518  UERROR("Updating pose scan %d failed!", iter->first);
2519  }
2520  }
2521  _cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
2522  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
2523  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
2524  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
2525  }
2526  else if(_cachedSignatures.contains(iter->first))
2527  {
2528  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2529  if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
2530  {
2531  this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2532  }
2533  }
2534  }
2535  else if(viewerClouds.contains(scanName))
2536  {
2537  _cloudViewer->setCloudVisibility(scanName.c_str(), false);
2538  }
2539 
2540  // occupancy grids
2541  bool updateGridMap =
2542  ((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
2543  (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
2544  _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
2545  bool updateOctomap = false;
2546 #ifdef RTABMAP_OCTOMAP
2547  updateOctomap =
2548  _cloudViewer->isVisible() &&
2549  _preferencesDialog->isOctomapUpdated() &&
2550  _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
2551 #endif
2552  if(updateGridMap || updateOctomap)
2553  {
2554  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2555  if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
2556  {
2557  cv::Mat ground;
2558  cv::Mat obstacles;
2559  cv::Mat empty;
2560 
2561  jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
2562 
2563  _occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
2564 
2565 #ifdef RTABMAP_OCTOMAP
2566  if(updateOctomap)
2567  {
2568  if((ground.empty() || ground.channels() > 2) &&
2569  (obstacles.empty() || obstacles.channels() > 2))
2570  {
2571  cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
2572  _octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
2573  }
2574  else if(!ground.empty() || !obstacles.empty())
2575  {
2576  UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
2577  }
2578  }
2579 #endif
2580  }
2581  }
2582 
2583  // 3d features
2584  std::string featuresName = uFormat("features%d", iter->first);
2585  if(iter->first == 0)
2586  {
2587  viewerClouds.remove(featuresName);
2588  _cloudViewer->removeCloud(featuresName);
2589  }
2590  if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
2591  {
2592  if(viewerClouds.contains(featuresName))
2593  {
2594  // Update only if the pose has changed
2595  Transform tFeatures;
2596  _cloudViewer->getPose(featuresName, tFeatures);
2597  if(tFeatures.isNull() || iter->second != tFeatures)
2598  {
2599  if(!_cloudViewer->updateCloudPose(featuresName, iter->second))
2600  {
2601  UERROR("Updating pose features %d failed!", iter->first);
2602  }
2603  }
2604  _cloudViewer->setCloudVisibility(featuresName, _preferencesDialog->isFeaturesShown(0));
2605  _cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
2606  }
2607  else if(_cachedSignatures.contains(iter->first))
2608  {
2609  QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
2610  if(!jter->getWords3().empty())
2611  {
2612  this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
2613  }
2614  }
2615  }
2616  else if(viewerClouds.contains(featuresName))
2617  {
2618  _cloudViewer->setCloudVisibility(featuresName.c_str(), false);
2619  }
2620 
2621  // Gravity arrows
2622  std::string gravityName = uFormat("gravity%d", iter->first);
2623  if(iter->first == 0)
2624  {
2625  viewerLines.erase(gravityName);
2626  _cloudViewer->removeLine(gravityName);
2627  }
2628  if(_cloudViewer->isVisible() && _preferencesDialog->isIMUGravityShown(0))
2629  {
2630  std::multimap<int, Link>::const_iterator linkIter = graph::findLink(constraints, iter->first, iter->first, false, Link::kGravity);
2631  if(linkIter != constraints.end())
2632  {
2633  Transform gravityT = linkIter->second.transform();
2634  Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0));
2635  gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity;
2636  _cloudViewer->addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.rotation().inverse(), Qt::yellow, false, false);
2637  }
2638  }
2639  else if(viewerLines.find(gravityName)!=viewerLines.end())
2640  {
2641  _cloudViewer->removeLine(gravityName.c_str());
2642  }
2643 
2644  if(verboseProgress)
2645  {
2646  _progressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
2647  _progressDialog->incrementStep();
2648  if(poses.size() < 200 || i % 100 == 0)
2649  {
2650  QApplication::processEvents();
2651  if(_progressCanceled)
2652  {
2653  break;
2654  }
2655  }
2656  }
2657  }
2658 
2659  ++i;
2660  }
2661 
2662  //remove not used clouds
2663  for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
2664  {
2665  std::list<std::string> splitted = uSplitNumChar(iter.key());
2666  int id = 0;
2667  if(splitted.size() == 2)
2668  {
2669  id = std::atoi(splitted.back().c_str());
2670  if(poses.find(id) == poses.end())
2671  {
2672  if(_cloudViewer->getCloudVisibility(iter.key()))
2673  {
2674  UDEBUG("Hide %s", iter.key().c_str());
2675  _cloudViewer->setCloudVisibility(iter.key(), false);
2676  }
2677  }
2678  }
2679  }
2680  // remove not used gravity lines
2681  for(std::set<std::string>::iterator iter = viewerLines.begin(); iter!=viewerLines.end(); ++iter)
2682  {
2683  std::list<std::string> splitted = uSplitNumChar(*iter);
2684  int id = 0;
2685  if(splitted.size() == 2)
2686  {
2687  id = std::atoi(splitted.back().c_str());
2688  if(poses.find(id) == poses.end())
2689  {
2690  UDEBUG("Remove %s", iter->c_str());
2691  _cloudViewer->removeLine(*iter);
2692  }
2693  }
2694  }
2695 
2696  UDEBUG("");
2697  if(stats)
2698  {
2699  stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f));
2700  }
2701 
2702  // update 3D graphes (show all poses)
2703  _cloudViewer->removeAllGraphs();
2704  _cloudViewer->removeCloud("graph_nodes");
2705  if(!_preferencesDialog->isFrustumsShown(0))
2706  {
2707  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
2708  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2709  {
2710  std::list<std::string> splitted = uSplitNumChar(iter.key());
2711  if(splitted.size() == 2)
2712  {
2713  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0))
2714  {
2715  _cloudViewer->removeFrustum(iter.key());
2716  }
2717  }
2718  }
2719  }
2720 
2721  Transform mapToGt = Transform::getIdentity();
2722  if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
2723  {
2724  mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
2725  }
2726 
2727  if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
2728  {
2729  UTimer timerGraph;
2730  // Find all graphs
2731  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
2732  for(std::map<int, Transform>::iterator iter=_currentPosesMap.lower_bound(1); iter!=_currentPosesMap.end(); ++iter)
2733  {
2734  int mapId = uValue(_currentMapIds, iter->first, -1);
2735 
2736  if(_preferencesDialog->isGraphsShown())
2737  {
2738  //edges
2739  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2740  if(kter == graphs.end())
2741  {
2742  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
2743  }
2744  pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
2745  kter->second->push_back(pt);
2746  }
2747 
2748  // get local transforms for frustums on the graph
2749  if(_preferencesDialog->isFrustumsShown(0))
2750  {
2751  std::string frustumId = uFormat("f_%d", iter->first);
2752  if(_cloudViewer->getAddedFrustums().contains(frustumId))
2753  {
2754  _cloudViewer->updateFrustumPose(frustumId, iter->second);
2755  }
2756  else if(_cachedSignatures.contains(iter->first))
2757  {
2758  const Signature & s = _cachedSignatures.value(iter->first);
2759  // Supporting only one frustum per node
2760  if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
2761  {
2762  const CameraModel & model = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().left():s.sensorData().cameraModels()[0];
2763  Transform t = model.localTransform();
2764  if(!t.isNull())
2765  {
2766  QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2767  _cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
2768 
2769  if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
2770  {
2771  std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
2772  color = Qt::gray;
2773  _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
2774  }
2775  }
2776  }
2777  }
2778  }
2779  }
2780 
2781  //Ground truth graph?
2782  for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
2783  {
2784  int mapId = -100;
2785 
2786  if(_preferencesDialog->isGraphsShown())
2787  {
2788  //edges
2789  std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2790  if(kter == graphs.end())
2791  {
2792  kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
2793  }
2794  Transform t = mapToGt*iter->second;
2795  pcl::PointXYZ pt(t.x(), t.y(), t.z());
2796  kter->second->push_back(pt);
2797  }
2798  }
2799 
2800  // add graphs
2801  for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
2802  {
2803  QColor color = Qt::gray;
2804  if(iter->first >= 0)
2805  {
2806  color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
2807  }
2808  _cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
2809  }
2810 
2811  if(_preferencesDialog->isFrustumsShown(0))
2812  {
2813  QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
2814  UDEBUG("remove not used frustums");
2815  for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2816  {
2817  std::list<std::string> splitted = uSplitNumChar(iter.key());
2818  if(splitted.size() == 2)
2819  {
2820  int id = std::atoi(splitted.back().c_str());
2821  if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0) &&
2822  _currentPosesMap.find(id) == _currentPosesMap.end())
2823  {
2824  _cloudViewer->removeFrustum(iter.key());
2825  }
2826  }
2827  }
2828  }
2829 
2830  UDEBUG("timerGraph=%fs", timerGraph.ticks());
2831  }
2832 
2833  UDEBUG("labels.size()=%d", (int)labels.size());
2834 
2835  // Update labels
2836  _cloudViewer->removeAllTexts();
2837  if(_preferencesDialog->isLabelsShown() && labels.size())
2838  {
2839  for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
2840  {
2841  if(nodePoses.find(iter->first)!=nodePoses.end())
2842  {
2843  int mapId = uValue(mapIdsIn, iter->first, -1);
2844  QColor color = Qt::gray;
2845  if(mapId >= 0)
2846  {
2847  color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2848  }
2849  _cloudViewer->addOrUpdateText(
2850  std::string("label_") + uNumber2Str(iter->first),
2851  iter->second,
2852  _currentPosesMap.at(iter->first),
2853  0.1,
2854  color);
2855  }
2856  }
2857  }
2858 
2859  UDEBUG("");
2860  if(stats)
2861  {
2862  stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f));
2863  }
2864 
2865 #ifdef RTABMAP_OCTOMAP
2866  _cloudViewer->removeOctomap();
2867  _cloudViewer->removeCloud("octomap_cloud");
2868  if(_preferencesDialog->isOctomapUpdated())
2869  {
2870  UDEBUG("");
2871  UTimer time;
2872  _octomap->update(poses);
2873  UINFO("Octomap update time = %fs", time.ticks());
2874  }
2875  if(stats)
2876  {
2877  stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f));
2878  }
2879  if(_preferencesDialog->isOctomapShown())
2880  {
2881  UDEBUG("");
2882  UTimer time;
2883  if(_preferencesDialog->getOctomapRenderingType() > 0)
2884  {
2885  _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
2886  }
2887  else
2888  {
2889  pcl::IndicesPtr obstacles(new std::vector<int>);
2890  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = _octomap->createCloud(_preferencesDialog->getOctomapTreeDepth(), obstacles.get());
2891  if(obstacles->size())
2892  {
2893  _cloudViewer->addCloud("octomap_cloud", cloud);
2894  _cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize());
2895  }
2896  }
2897  UINFO("Octomap show 3d map time = %fs", time.ticks());
2898  }
2899  UDEBUG("");
2900  if(stats)
2901  {
2902  stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f));
2903  }
2904 #endif
2905 
2906  // Add landmarks to 3D Map view
2907 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2908  _cloudViewer->removeAllCoordinates("landmark_");
2909 #endif
2910  if(_preferencesDialog->isLandmarksShown())
2911  {
2912  for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end() && iter->first<0; ++iter)
2913  {
2914 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
2915  _cloudViewer->addOrUpdateCoordinate(uFormat("landmark_%d", -iter->first), iter->second, _preferencesDialog->landmarkVisSize()>0.0?_preferencesDialog->landmarkVisSize():_preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength()/2.0, false);
2916 #endif
2917  if(_preferencesDialog->isLabelsShown())
2918  {
2919  std::string num = uNumber2Str(-iter->first);
2920  _cloudViewer->addOrUpdateText(
2921  std::string("landmark_str_") + num,
2922  num,
2923  iter->second,
2924  0.1,
2925  Qt::yellow);
2926  }
2927  }
2928  }
2929 
2930  // Update occupancy grid map in 3D map view and graph view
2931  if(_ui->graphicsView_graphView->isVisible())
2932  {
2933  _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
2934  if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
2935  {
2936  std::map<int, Transform> gtPoses = _currentGTPosesMap;
2937  for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
2938  {
2939  iter->second = mapToGt * iter->second;
2940  }
2941  _ui->graphicsView_graphView->updateGTGraph(gtPoses);
2942  }
2943  else
2944  {
2945  _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
2946  }
2947  }
2948  cv::Mat map8U;
2949  if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
2950  {
2951  float xMin, yMin;
2952  float resolution = _occupancyGrid->getCellSize();
2953  cv::Mat map8S;
2954 #ifdef RTABMAP_OCTOMAP
2955  if(_preferencesDialog->isOctomap2dGrid())
2956  {
2957  map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth());
2958 
2959  }
2960  else
2961 #endif
2962  {
2963  if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0)
2964  {
2965  _occupancyGrid->update(poses);
2966  }
2967  if(stats)
2968  {
2969  stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
2970  }
2971  map8S = _occupancyGrid->getMap(xMin, yMin);
2972  }
2973  if(!map8S.empty())
2974  {
2975  //convert to gray scaled map
2976  map8U = util3d::convertMap2Image8U(map8S);
2977 
2978  if(_preferencesDialog->getGridMapShown())
2979  {
2980  float opacity = _preferencesDialog->getGridMapOpacity();
2981  _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
2982  }
2983  if(_ui->graphicsView_graphView->isVisible())
2984  {
2985  _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
2986  }
2987  }
2988  }
2989  _ui->graphicsView_graphView->update();
2990 
2991  UDEBUG("");
2992  if(stats)
2993  {
2994  stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f));
2995  }
2996 
2997  if(!_preferencesDialog->getGridMapShown())
2998  {
2999  UDEBUG("");
3000  _cloudViewer->removeOccupancyGridMap();
3001  }
3002 
3003  if(viewerClouds.contains("cloudOdom"))
3004  {
3005  if(!_preferencesDialog->isCloudsShown(1))
3006  {
3007  UDEBUG("");
3008  _cloudViewer->setCloudVisibility("cloudOdom", false);
3009  }
3010  else
3011  {
3012  UDEBUG("");
3013  _cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
3014  _cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
3015  _cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
3016  _cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
3017  }
3018  }
3019  if(viewerClouds.contains("scanOdom"))
3020  {
3021  if(!_preferencesDialog->isScansShown(1))
3022  {
3023  UDEBUG("");
3024  _cloudViewer->setCloudVisibility("scanOdom", false);
3025  }
3026  else
3027  {
3028  UDEBUG("");
3029  _cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
3030  _cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
3031  _cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
3032  _cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
3033  }
3034  }
3035  if(viewerClouds.contains("scanMapOdom"))
3036  {
3037  if(!_preferencesDialog->isScansShown(1))
3038  {
3039  UDEBUG("");
3040  _cloudViewer->setCloudVisibility("scanMapOdom", false);
3041  }
3042  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
3043  {
3044  UDEBUG("");
3045  _cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
3046  _cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
3047  _cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
3048  _cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
3049  }
3050  }
3051  if(viewerClouds.contains("featuresOdom"))
3052  {
3053  if(!_preferencesDialog->isFeaturesShown(1))
3054  {
3055  UDEBUG("");
3056  _cloudViewer->setCloudVisibility("featuresOdom", false);
3057  }
3058  else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
3059  {
3060  UDEBUG("");
3061  _cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
3062  _cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
3063  }
3064  }
3065 
3066  // activate actions
3067  if(_state != kMonitoring && _state != kDetecting)
3068  {
3069  _ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
3070  _ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
3071  _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_occupancyGrid->addedNodes().empty());
3072 #ifdef RTABMAP_OCTOMAP
3073  _ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
3074 #else
3075  _ui->actionExport_octomap->setEnabled(false);
3076 #endif
3077  }
3078 
3079  UDEBUG("");
3080  _cloudViewer->update();
3081  UDEBUG("");
3082 }
3083 
3084 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
3085 {
3086  UASSERT(!pose.isNull());
3087  std::string cloudName = uFormat("cloud%d", nodeId);
3088  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
3089  if(_cloudViewer->getAddedClouds().contains(cloudName))
3090  {
3091  UERROR("Cloud %d already added to map.", nodeId);
3092  return outputPair;
3093  }
3094 
3095  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3096  if(iter == _cachedSignatures.end())
3097  {
3098  UERROR("Node %d is not in the cache.", nodeId);
3099  return outputPair;
3100  }
3101 
3102  UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
3103 
3104  if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
3105  (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
3106  {
3107  cv::Mat image, depth;
3108  SensorData data = iter->sensorData();
3109  data.uncompressData(&image, &depth, 0);
3110  ParametersMap allParameters = _preferencesDialog->getAllParameters();
3111  bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
3112  bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
3113  Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
3114  Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
3115  if(rectifyOnlyFeatures && !imagesAlreadyRectified)
3116  {
3117  if(data.cameraModels().size())
3118  {
3119  UTimer time;
3120  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
3121  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
3122  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
3123  cv::Mat rectifiedImages = data.imageRaw().clone();
3124  bool initRectMaps = _rectCameraModels.empty();
3125  if(initRectMaps)
3126  {
3127  _rectCameraModels.resize(data.cameraModels().size());
3128  }
3129  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
3130  {
3131  if(data.cameraModels()[i].isValidForRectification())
3132  {
3133  if(initRectMaps)
3134  {
3135  _rectCameraModels[i] = data.cameraModels()[i];
3136  if(!_rectCameraModels[i].isRectificationMapInitialized())
3137  {
3138  UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
3139  _rectCameraModels[i].initRectificationMap();
3140  UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
3141  }
3142  }
3143  UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
3144  _rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
3145  cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3146  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
3147  }
3148  else
3149  {
3150  UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
3151  i, data.id(),
3152  data.cameraModels()[i].imageWidth(),
3153  data.cameraModels()[i].imageHeight());
3154  }
3155  }
3156  UINFO("Time rectification: %fs", time.ticks());
3157  data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
3158  image = rectifiedImages;
3159  }
3160  }
3161 
3162  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
3163  pcl::IndicesPtr indices(new std::vector<int>);
3164  UASSERT_MSG(nodeId == 0 || nodeId == data.id(), uFormat("nodeId=%d data.id()=%d", nodeId, data.id()).c_str());
3165 
3166  // Create organized cloud
3167  cloud = util3d::cloudRGBFromSensorData(data,
3168  _preferencesDialog->getCloudDecimation(0),
3169  _preferencesDialog->getCloudMaxDepth(0),
3170  _preferencesDialog->getCloudMinDepth(0),
3171  indices.get(),
3172  allParameters,
3173  _preferencesDialog->getCloudRoiRatios(0));
3174 
3175  // view point
3176  Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
3177  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
3178  {
3179  viewPoint[0] = data.cameraModels()[0].localTransform().x();
3180  viewPoint[1] = data.cameraModels()[0].localTransform().y();
3181  viewPoint[2] = data.cameraModels()[0].localTransform().z();
3182  }
3183  else if(!data.stereoCameraModel().localTransform().isNull())
3184  {
3185  viewPoint[0] = data.stereoCameraModel().localTransform().x();
3186  viewPoint[1] = data.stereoCameraModel().localTransform().y();
3187  viewPoint[2] = data.stereoCameraModel().localTransform().z();
3188  }
3189 
3190  // filtering pipeline
3191  if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
3192  {
3193  cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
3194  //generate indices for all points (they are all valid)
3195  indices->resize(cloud->size());
3196  for(unsigned int i=0; i<cloud->size(); ++i)
3197  {
3198  indices->at(i) = i;
3199  }
3200  }
3201 
3202  // Do ceiling/floor filtering
3203  if(indices->size() &&
3204  (_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
3205  _preferencesDialog->getCeilingFilteringHeight() != 0.0))
3206  {
3207  // perform in /map frame
3208  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
3209  indices = rtabmap::util3d::passThrough(
3210  cloudTransformed,
3211  indices,
3212  "z",
3213  _preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
3214  _preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
3215  }
3216 
3217  // Do radius filtering after voxel filtering ( a lot faster)
3218  if(indices->size() &&
3219  _preferencesDialog->getNoiseRadius() > 0.0 &&
3220  _preferencesDialog->getNoiseMinNeighbors() > 0)
3221  {
3222  indices = rtabmap::util3d::radiusFiltering(
3223  cloud,
3224  indices,
3225  _preferencesDialog->getNoiseRadius(),
3226  _preferencesDialog->getNoiseMinNeighbors());
3227  }
3228 
3229  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3230  if(_preferencesDialog->isSubtractFiltering() &&
3231  _preferencesDialog->getSubtractFilteringRadius() > 0.0 &&
3232  nodeId > 0)
3233  {
3234  pcl::IndicesPtr beforeFiltering = indices;
3235  if( cloud->size() &&
3236  _previousCloud.first>0 &&
3237  _previousCloud.second.first.first.get() != 0 &&
3238  _previousCloud.second.second.get() != 0 &&
3239  _previousCloud.second.second->size() &&
3240  _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
3241  {
3242  UTimer time;
3243 
3244  rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
3245 
3246  //UWARN("saved new.pcd and old.pcd");
3247  //pcl::io::savePCDFile("new.pcd", *cloud, *indices);
3248  //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
3249 
3250  if(_preferencesDialog->isSubtractFiltering())
3251  {
3252  if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
3253  {
3254  //normals required
3255  if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
3256  {
3257  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
3258  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3259  }
3260  else
3261  {
3262  UWARN("Cloud subtraction with angle filtering is activated but "
3263  "cloud normal K search is 0. Subtraction is done with angle.");
3264  }
3265  }
3266 
3267  if(cloudWithNormals->size() &&
3268  _previousCloud.second.first.second.get() &&
3269  _previousCloud.second.first.second->size())
3270  {
3271  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t);
3272  indices = rtabmap::util3d::subtractFiltering(
3273  cloudWithNormals,
3274  indices,
3275  previousCloud,
3276  _previousCloud.second.second,
3277  _preferencesDialog->getSubtractFilteringRadius(),
3278  _preferencesDialog->getSubtractFilteringAngle(),
3279  _preferencesDialog->getSubtractFilteringMinPts());
3280  }
3281  else
3282  {
3283  pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t);
3284  indices = rtabmap::util3d::subtractFiltering(
3285  cloud,
3286  indices,
3287  previousCloud,
3288  _previousCloud.second.second,
3289  _preferencesDialog->getSubtractFilteringRadius(),
3290  _preferencesDialog->getSubtractFilteringMinPts());
3291  }
3292 
3293 
3294  UINFO("Time subtract filtering %d from %d -> %d (%fs)",
3295  (int)_previousCloud.second.second->size(),
3296  (int)beforeFiltering->size(),
3297  (int)indices->size(),
3298  time.ticks());
3299  }
3300  }
3301  // keep all indices for next subtraction
3302  _previousCloud.first = nodeId;
3303  _previousCloud.second.first.first = cloud;
3304  _previousCloud.second.first.second = cloudWithNormals;
3305  _previousCloud.second.second = beforeFiltering;
3306  }
3307 
3308  if(indices->size())
3309  {
3310  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
3311  bool added = false;
3312  if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
3313  {
3314  // Fast organized mesh
3315  // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
3316  output = util3d::extractIndices(cloud, indices, false, true);
3317  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
3318  output,
3319  _preferencesDialog->getCloudMeshingAngle(),
3320  _preferencesDialog->isCloudMeshingQuad(),
3321  _preferencesDialog->getCloudMeshingTriangleSize(),
3322  viewPoint);
3323  if(polygons.size())
3324  {
3325  // remove unused vertices to save memory
3326  pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
3327  std::vector<pcl::Vertices> outputPolygons;
3328  std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
3329 
3330  if(_preferencesDialog->isCloudMeshingTexture() && !image.empty())
3331  {
3332  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
3333  pcl::toPCLPointCloud2(*outputFiltered, textureMesh->cloud);
3334  textureMesh->tex_polygons.push_back(outputPolygons);
3335  int w = cloud->width;
3336  int h = cloud->height;
3337  UASSERT(w > 1 && h > 1);
3338  textureMesh->tex_coordinates.resize(1);
3339  int nPoints = (int)outputFiltered->size();
3340  textureMesh->tex_coordinates[0].resize(nPoints);
3341  for(int i=0; i<nPoints; ++i)
3342  {
3343  //uv
3344  UASSERT(i < (int)denseToOrganizedIndices.size());
3345  int originalVertex = denseToOrganizedIndices[i];
3346  textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
3347  float(originalVertex % w) / float(w), // u
3348  float(h - originalVertex / w) / float(h)); // v
3349  }
3350 
3351  pcl::TexMaterial mesh_material;
3352  mesh_material.tex_d = 1.0f;
3353  mesh_material.tex_Ns = 75.0f;
3354  mesh_material.tex_illum = 1;
3355 
3356  std::stringstream tex_name;
3357  tex_name << "material_" << nodeId;
3358  tex_name >> mesh_material.tex_name;
3359 
3360  mesh_material.tex_file = "";
3361 
3362  textureMesh->tex_materials.push_back(mesh_material);
3363 
3364  if(!_cloudViewer->addCloudTextureMesh(cloudName, textureMesh, image, pose))
3365  {
3366  UERROR("Adding texture mesh %d to viewer failed!", nodeId);
3367  }
3368  else
3369  {
3370  added = true;
3371  }
3372  }
3373  else if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
3374  {
3375  UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
3376  }
3377  else
3378  {
3379  added = true;
3380  }
3381  }
3382  }
3383  else
3384  {
3385  if(_preferencesDialog->isCloudMeshing())
3386  {
3387  UWARN("Online meshing is activated but the generated cloud is "
3388  "dense (voxel filtering is used or multiple cameras are used). Disable "
3389  "online meshing in Preferences->3D Rendering to hide this warning.");
3390  }
3391 
3392  if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
3393  {
3394  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
3395  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3396  }
3397 
3398  QColor color = Qt::gray;
3399  if(mapId >= 0)
3400  {
3401  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3402  }
3403 
3404  output = util3d::extractIndices(cloud, indices, false, true);
3405 
3406  if(cloudWithNormals->size())
3407  {
3408  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
3409  outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false);
3410 
3411  if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color))
3412  {
3413  UERROR("Adding cloud %d to viewer failed!", nodeId);
3414  }
3415  else
3416  {
3417  added = true;
3418  }
3419  }
3420  else
3421  {
3422  if(!_cloudViewer->addCloud(cloudName, output, pose, color))
3423  {
3424  UERROR("Adding cloud %d to viewer failed!", nodeId);
3425  }
3426  else
3427  {
3428  added = true;
3429  }
3430  }
3431  }
3432  if(added)
3433  {
3434  outputPair.first = output;
3435  outputPair.second = indices;
3436  if(_preferencesDialog->isCloudsKept() && nodeId > 0)
3437  {
3438  _cachedClouds.insert(std::make_pair(nodeId, outputPair));
3439  _createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
3440  }
3441  _cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
3442  _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
3443  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
3444  }
3445  else if(nodeId>0)
3446  {
3447  _cachedEmptyClouds.insert(nodeId);
3448  }
3449  }
3450  else if(nodeId>0)
3451  {
3452  _cachedEmptyClouds.insert(nodeId);
3453  }
3454  }
3455 
3456  return outputPair;
3457 }
3458 
3459 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
3460 {
3461  std::string scanName = uFormat("scan%d", nodeId);
3462  if(_cloudViewer->getAddedClouds().contains(scanName))
3463  {
3464  UERROR("Scan %d already added to map.", nodeId);
3465  return;
3466  }
3467 
3468  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3469  if(iter == _cachedSignatures.end())
3470  {
3471  UERROR("Node %d is not in the cache.", nodeId);
3472  return;
3473  }
3474 
3475  if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
3476  {
3477  LaserScan scan;
3478  iter->sensorData().uncompressData(0, 0, &scan);
3479 
3480  if(_preferencesDialog->getDownsamplingStepScan(0) > 1 ||
3481  _preferencesDialog->getScanMaxRange(0) > 0.0f ||
3482  _preferencesDialog->getScanMinRange(0) > 0.0f)
3483  {
3484  scan = util3d::commonFiltering(scan,
3485  _preferencesDialog->getDownsamplingStepScan(0),
3486  _preferencesDialog->getScanMinRange(0),
3487  _preferencesDialog->getScanMaxRange(0));
3488  }
3489 
3490  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
3491  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
3492  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
3493  pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
3494  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
3495  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
3496  if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3497  {
3498  cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
3499  }
3500  else if(scan.hasNormals() && scan.hasIntensity() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3501  {
3502  cloudIWithNormals = util3d::laserScanToPointCloudINormal(scan, scan.localTransform());
3503  }
3504  else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
3505  {
3506  cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
3507  }
3508  else if(scan.hasRGB())
3509  {
3510  cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
3511  }
3512  else if(scan.hasIntensity())
3513  {
3514  cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
3515  }
3516  else
3517  {
3518  cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
3519  }
3520 
3521  if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
3522  {
3523  if(cloud.get())
3524  {
3525  cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
3526  }
3527  if(cloudRGB.get())
3528  {
3529  cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
3530  }
3531  if(cloudI.get())
3532  {
3533  cloudI = util3d::voxelize(cloudI, _preferencesDialog->getCloudVoxelSizeScan(0));
3534  }
3535  }
3536 
3537  // Do ceiling/floor filtering
3538  if((!scan.is2d()) && // don't filter 2D scans
3539  (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
3540  _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
3541  {
3542  if(cloudRGBWithNormals.get())
3543  {
3544  // perform in /map frame
3545  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
3546  cloudTransformed = rtabmap::util3d::passThrough(
3547  cloudTransformed,
3548  "z",
3549  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3550  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3551 
3552  //transform back in sensor frame
3553  cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3554  }
3555  if(cloudIWithNormals.get())
3556  {
3557  // perform in /map frame
3558  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudIWithNormals, pose);
3559  cloudTransformed = rtabmap::util3d::passThrough(
3560  cloudTransformed,
3561  "z",
3562  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3563  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3564 
3565  //transform back in sensor frame
3566  cloudIWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3567  }
3568  if(cloudWithNormals.get())
3569  {
3570  // perform in /map frame
3571  pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
3572  cloudTransformed = rtabmap::util3d::passThrough(
3573  cloudTransformed,
3574  "z",
3575  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3576  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3577 
3578  //transform back in sensor frame
3579  cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3580  }
3581  if(cloudRGB.get())
3582  {
3583  // perform in /map frame
3584  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
3585  cloudTransformed = rtabmap::util3d::passThrough(
3586  cloudTransformed,
3587  "z",
3588  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3589  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3590 
3591  //transform back in sensor frame
3592  cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3593  }
3594  if(cloudI.get())
3595  {
3596  // perform in /map frame
3597  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudTransformed = util3d::transformPointCloud(cloudI, pose);
3598  cloudTransformed = rtabmap::util3d::passThrough(
3599  cloudTransformed,
3600  "z",
3601  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3602  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3603 
3604  //transform back in sensor frame
3605  cloudI = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3606  }
3607  if(cloud.get())
3608  {
3609  // perform in /map frame
3610  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
3611  cloudTransformed = rtabmap::util3d::passThrough(
3612  cloudTransformed,
3613  "z",
3614  _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
3615  _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
3616 
3617  //transform back in sensor frame
3618  cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
3619  }
3620  }
3621 
3622  if( (cloud.get() || cloudRGB.get() || cloudI.get()) &&
3623  (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
3624  {
3625  Eigen::Vector3f scanViewpoint(
3626  scan.localTransform().x(),
3627  scan.localTransform().y(),
3628  scan.localTransform().z());
3629 
3630  pcl::PointCloud<pcl::Normal>::Ptr normals;
3631  if(cloud.get() && cloud->size())
3632  {
3633  if(scan.is2d())
3634  {
3635  normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3636  }
3637  else
3638  {
3639  normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3640  }
3641  cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
3642  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3643  cloud.reset();
3644  }
3645  else if(cloudRGB.get() && cloudRGB->size())
3646  {
3647  // Assuming 3D
3648  normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3649  cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3650  pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
3651  cloudRGB.reset();
3652  }
3653  else if(cloudI.get())
3654  {
3655  if(scan.is2d())
3656  {
3657  normals = util3d::computeFastOrganizedNormals2D(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3658  }
3659  else
3660  {
3661  normals = util3d::computeNormals(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
3662  }
3663  cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
3664  pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
3665  cloudI.reset();
3666  }
3667  }
3668 
3669  QColor color = Qt::gray;
3670  if(mapId >= 0)
3671  {
3672  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3673  }
3674  bool added = false;
3675  if(cloudRGBWithNormals.get())
3676  {
3677  added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
3678  if(added && nodeId > 0)
3679  {
3680  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
3681  }
3682  }
3683  else if(cloudIWithNormals.get())
3684  {
3685  added = _cloudViewer->addCloud(scanName, cloudIWithNormals, pose, color);
3686  if(added && nodeId > 0)
3687  {
3688  if(scan.is2d())
3689  {
3690  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
3691  }
3692  else
3693  {
3694  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
3695  }
3696  }
3697  }
3698  else if(cloudWithNormals.get())
3699  {
3700  added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
3701  if(added && nodeId > 0)
3702  {
3703  if(scan.is2d())
3704  {
3705  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
3706  }
3707  else
3708  {
3709  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
3710  }
3711  }
3712  }
3713  else if(cloudRGB.get())
3714  {
3715  added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
3716  if(added && nodeId > 0)
3717  {
3718  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
3719  }
3720  }
3721  else if(cloudI.get())
3722  {
3723  added = _cloudViewer->addCloud(scanName, cloudI, pose, color);
3724  if(added && nodeId > 0)
3725  {
3726  if(scan.is2d())
3727  {
3728  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
3729  }
3730  else
3731  {
3732  scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
3733  }
3734  }
3735  }
3736  else
3737  {
3738  UASSERT(cloud.get());
3739  added = _cloudViewer->addCloud(scanName, cloud, pose, color);
3740  if(added && nodeId > 0)
3741  {
3742  if(scan.is2d())
3743  {
3744  scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXY, scan.localTransform());
3745  }
3746  else
3747  {
3748  scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
3749  }
3750  }
3751  }
3752  if(!added)
3753  {
3754  UERROR("Adding cloud %d to viewer failed!", nodeId);
3755  }
3756  else
3757  {
3758  if(nodeId > 0)
3759  {
3760  _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in scan frame
3761  }
3762 
3763  _cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(0));
3764  _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
3765  _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
3766  }
3767  }
3768 }
3769 
3770 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
3771 {
3772  UDEBUG("");
3773  UASSERT(!pose.isNull());
3774  std::string cloudName = uFormat("features%d", nodeId);
3775  if(_cloudViewer->getAddedClouds().contains(cloudName))
3776  {
3777  UERROR("Features cloud %d already added to map.", nodeId);
3778  return;
3779  }
3780 
3781  QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
3782  if(iter == _cachedSignatures.end())
3783  {
3784  UERROR("Node %d is not in the cache.", nodeId);
3785  return;
3786  }
3787 
3788  if(_createdFeatures.find(nodeId) != _createdFeatures.end())
3789  {
3790  UDEBUG("Features cloud %d already created.");
3791  return;
3792  }
3793 
3794  if(iter->getWords3().size())
3795  {
3796  UINFO("Create cloud from 3D words");
3797  QColor color = Qt::gray;
3798  if(mapId >= 0)
3799  {
3800  color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3801  }
3802 
3803  cv::Mat rgb;
3804  if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
3805  {
3806  SensorData data = iter->sensorData();
3807  data.uncompressData(&rgb, 0, 0);
3808  }
3809 
3810  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3811  cloud->resize(iter->getWords3().size());
3812  int oi=0;
3813  UASSERT(iter->getWords().size() == iter->getWords3().size());
3814  float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
3815  UDEBUG("rgb.channels()=%d");
3816  if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
3817  {
3818  Transform invLocalTransform = Transform::getIdentity();
3819  if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
3820  {
3821  invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
3822  }
3823  else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
3824  {
3825  invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().inverse();
3826  }
3827 
3828  for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
3829  {
3830  const cv::Point3f & pt = iter->getWords3()[jter->second];
3831  if(util3d::isFinite(pt) &&
3832  (maxDepth == 0.0f ||
3833  //move back point in camera frame (to get depth along z), ignore for multi-camera
3834  (iter.value().sensorData().cameraModels().size()<=1 &&
3835  util3d::transformPoint(pt, invLocalTransform).z < maxDepth)))
3836  {
3837  (*cloud)[oi].x = pt.x;
3838  (*cloud)[oi].y = pt.y;
3839  (*cloud)[oi].z = pt.z;
3840  const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
3841  int u = kpt.pt.x+0.5;
3842  int v = kpt.pt.y+0.5;
3843  if(!rgb.empty() &&
3844  uIsInBounds(u, 0, rgb.cols-1) &&
3845  uIsInBounds(v, 0, rgb.rows-1))
3846  {
3847  if(rgb.channels() == 1)
3848  {
3849  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
3850  }
3851  else
3852  {
3853  cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
3854  (*cloud)[oi].b = bgr.val[0];
3855  (*cloud)[oi].g = bgr.val[1];
3856  (*cloud)[oi].r = bgr.val[2];
3857  }
3858  }
3859  else
3860  {
3861  (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
3862  }
3863  ++oi;
3864  }
3865  }
3866  }
3867  cloud->resize(oi);
3868  if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
3869  {
3870  UERROR("Adding features cloud %d to viewer failed!", nodeId);
3871  }
3872  else if(nodeId > 0)
3873  {
3874  _createdFeatures.insert(std::make_pair(nodeId, cloud));
3875  }
3876  }
3877  else
3878  {
3879  return;
3880  }
3881 
3882  _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
3883  UDEBUG("");
3884 }
3885 
3886 Transform MainWindow::alignPosesToGroundTruth(
3887  const std::map<int, Transform> & poses,
3888  const std::map<int, Transform> & groundTruth)
3889 {
3890  Transform t = Transform::getIdentity();
3891  if(groundTruth.size() && poses.size())
3892  {
3893  float translational_rmse = 0.0f;
3894  float translational_mean = 0.0f;
3895  float translational_median = 0.0f;
3896  float translational_std = 0.0f;
3897  float translational_min = 0.0f;
3898  float translational_max = 0.0f;
3899  float rotational_rmse = 0.0f;
3900  float rotational_mean = 0.0f;
3901  float rotational_median = 0.0f;
3902  float rotational_std = 0.0f;
3903  float rotational_min = 0.0f;
3904  float rotational_max = 0.0f;
3905 
3906  t = graph::calcRMSE(
3907  groundTruth,
3908  poses,
3909  translational_rmse,
3910  translational_mean,
3911  translational_median,
3912  translational_std,
3913  translational_min,
3914  translational_max,
3915  rotational_rmse,
3916  rotational_mean,
3917  rotational_median,
3918  rotational_std,
3919  rotational_min,
3920  rotational_max);
3921 
3922  // ground truth live statistics
3923  UINFO("translational_rmse=%f", translational_rmse);
3924  UINFO("translational_mean=%f", translational_mean);
3925  UINFO("translational_median=%f", translational_median);
3926  UINFO("translational_std=%f", translational_std);
3927  UINFO("translational_min=%f", translational_min);
3928  UINFO("translational_max=%f", translational_max);
3929 
3930  UINFO("rotatio