MainWindow.h
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 #ifndef RTABMAP_MAINWINDOW_H_
29 #define RTABMAP_MAINWINDOW_H_
30 
31 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
32 
34 #include <QMainWindow>
35 #include <QtCore/QSet>
41 
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 #include <pcl/PolygonMesh.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/TextureMesh.h>
47 
48 namespace rtabmap {
49 class CameraThread;
50 class OdometryThread;
51 class IMUThread;
52 class CloudViewer;
53 class LoopClosureViewer;
54 class OccupancyGrid;
55 }
56 
57 class QGraphicsScene;
58 class Ui_mainWindow;
59 class QActionGroup;
60 
61 namespace rtabmap {
62 
63 class LikelihoodScene;
64 class AboutDialog;
65 class Plot;
66 class PdfPlotCurve;
67 class StatsToolBox;
68 class ProgressDialog;
69 class TwistGridWidget;
70 class ExportCloudsDialog;
71 class ExportBundlerDialog;
72 class PostProcessingDialog;
73 class DepthCalibrationDialog;
74 class DataRecorder;
75 class OctoMap;
76 
77 class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
78 {
79  Q_OBJECT
80 
81 public:
82  enum State {
92  kMonitoringPaused
93  };
94 
95 public:
100  MainWindow(PreferencesDialog * prefDialog = 0, QWidget * parent = 0, bool showSplashScreen = true);
101  virtual ~MainWindow();
102 
103  QString getWorkingDirectory() const;
104  void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
105  bool isSavedMaximized() const {return _savedMaximized;}
106 
107  bool isProcessingStatistics() const {return _processingStatistics;}
108  bool isProcessingOdometry() const {return _processingOdometry;}
109 
110  bool isDatabaseUpdated() const { return _databaseUpdated; }
111 
112 public Q_SLOTS:
113  virtual void processStats(const rtabmap::Statistics & stat);
114  void updateCacheFromDatabase(const QString & path);
115  void openDatabase(const QString & path, const rtabmap::ParametersMap & overridedParameters = rtabmap::ParametersMap());
116  void updateParameters(const rtabmap::ParametersMap & parameters);
117 
118 protected:
119  virtual void closeEvent(QCloseEvent* event);
120  virtual bool handleEvent(UEvent* anEvent);
121  virtual void showEvent(QShowEvent* anEvent);
122  virtual void moveEvent(QMoveEvent* anEvent);
123  virtual void resizeEvent(QResizeEvent* anEvent);
124  virtual void keyPressEvent(QKeyEvent *event);
125  virtual bool eventFilter(QObject *obj, QEvent *event);
126 
127 protected Q_SLOTS:
128  virtual void changeState(MainWindow::State state);
129  virtual void newDatabase();
130  virtual void openDatabase();
131  virtual bool closeDatabase();
132  virtual void startDetection();
133  virtual void pauseDetection();
134  virtual void stopDetection();
135  virtual void saveConfigGUI();
136  virtual void downloadAllClouds();
137  virtual void downloadPoseGraph();
138  virtual void clearTheCache();
139  virtual void openHelp();
140  virtual void openPreferences();
141  virtual void openPreferencesSource();
142  virtual void setDefaultViews();
143  virtual void resetOdometry();
144  virtual void triggerNewMap();
145  virtual void deleteMemory();
146 
147 protected Q_SLOTS:
148  void beep();
149  void cancelProgress();
150  void configGUIModified();
151  void editDatabase();
152  void notifyNoMoreImages();
153  void printLoopClosureIds();
154  void generateGraphDOT();
155  void exportPosesRaw();
156  void exportPosesRGBDSLAM();
157  void exportPosesRGBDSLAMMotionCapture();
158  void exportPosesKITTI();
159  void exportPosesTORO();
160  void exportPosesG2O();
161  void exportImages();
162  void exportOctomap();
163  void postProcessing();
164  void depthCalibration();
165  void openWorkingDirectory();
166  void updateEditMenu();
167  void selectStream();
168  void selectOpenni();
169  void selectFreenect();
170  void selectOpenniCv();
171  void selectOpenniCvAsus();
172  void selectOpenni2();
173  void selectFreenect2();
174  void selectK4W2();
175  void selectK4A();
176  void selectRealSense();
177  void selectRealSense2();
178  void selectRealSense2Stereo();
179  void selectStereoDC1394();
180  void selectStereoFlyCapture2();
181  void selectStereoZed();
182  void selectStereoTara();
183  void selectStereoUsb();
184  void selectMyntEyeS();
185  void dumpTheMemory();
186  void dumpThePrediction();
187  void sendGoal();
188  void sendWaypoints();
189  void postGoal(const QString & goal);
190  void cancelGoal();
191  void label();
192  void updateCacheFromDatabase();
193  void anchorCloudsToGroundTruth();
194  void selectScreenCaptureFormat(bool checked);
195  void takeScreenshot();
196  void updateElapsedTime();
197  void processCameraInfo(const rtabmap::CameraInfo & info);
198  void processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored);
199  void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
200  void applyPrefSettings(const rtabmap::ParametersMap & parameters);
201  void processRtabmapEventInit(int status, const QString & info);
202  void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
203  void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
204  void processRtabmapLabelErrorEvent(int id, const QString & label);
205  void processRtabmapGoalStatusEvent(int status);
206  void changeImgRateSetting();
207  void changeDetectionRateSetting();
208  void changeTimeLimitSetting();
209  void changeMappingMode();
210  void setAspectRatio(int w, int h);
211  void setAspectRatio16_9();
212  void setAspectRatio16_10();
213  void setAspectRatio4_3();
214  void setAspectRatio240p();
215  void setAspectRatio360p();
216  void setAspectRatio480p();
217  void setAspectRatio720p();
218  void setAspectRatio1080p();
219  void setAspectRatioCustom();
220  void exportGridMap();
221  void exportClouds();
222  void exportBundlerFormat();
223  void viewClouds();
224  void dataRecorder();
225  void dataRecorderDestroyed();
226  void updateNodeVisibility(int, bool);
227  void updateGraphView();
228 
229 Q_SIGNALS:
230  void statsReceived(const rtabmap::Statistics &);
231  void statsProcessed();
232  void cameraInfoReceived(const rtabmap::CameraInfo &);
233  void cameraInfoProcessed();
234  void odometryReceived(const rtabmap::OdometryEvent &, bool);
235  void odometryProcessed();
236  void thresholdsChanged(int, int);
237  void stateChanged(MainWindow::State);
238  void rtabmapEventInitReceived(int status, const QString & info);
239  void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
240  void rtabmapEvent3DMapProcessed();
241  void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
242  void rtabmapLabelErrorReceived(int id, const QString & label);
243  void rtabmapGoalStatusEventReceived(int status);
244  void imgRateChanged(double);
245  void detectionRateChanged(double);
246  void timeLimitChanged(float);
247  void mappingModeChanged(bool);
248  void noMoreImagesReceived();
249  void loopClosureThrChanged(qreal);
250  void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
251 
252 private:
253  void update3DMapVisibility(bool cloudsShown, bool scansShown);
254  void updateMapCloud(
255  const std::map<int, Transform> & poses,
256  const std::multimap<int, Link> & constraints,
257  const std::map<int, int> & mapIds,
258  const std::map<int, std::string> & labels,
259  const std::map<int, Transform> & groundTruths,
260  bool verboseProgress = false,
261  std::map<std::string, float> * stats = 0);
262  std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
263  void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
264  void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
265  Transform alignPosesToGroundTruth(const std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
266  void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
267  void drawLandmarks(cv::Mat & image, const Signature & signature);
268  void setupMainLayout(bool vertical);
269  void updateSelectSourceMenu();
270  void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
271  void saveFigures();
272  void loadFigures();
273  void exportPoses(int format);
274  QString captureScreen(bool cacheInRAM = false, bool png = true);
275 
276 protected:
277  Ui_mainWindow * ui() { return _ui; }
278  const State & state() const { return _state; }
279 
280  const QMap<int, Signature> & cachedSignatures() const { return _cachedSignatures;}
281  const std::map<int, Transform> & currentPosesMap() const { return _currentPosesMap; } // <nodeId, pose>
282  const std::map<int, Transform> & currentGTPosesMap() const { return _currentGTPosesMap; } // <nodeId, pose>
283  const std::multimap<int, Link> & currentLinksMap() const { return _currentLinksMap; } // <nodeFromId, link>
284  const std::map<int, int> & currentMapIds() const { return _currentMapIds; } // <nodeId, mapId>
285  const std::map<int, std::string> & currentLabels() const { return _currentLabels; } // <nodeId, label>
286  const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds() const { return _cachedClouds; }
287  const std::map<int, LaserScan> & createdScans() const { return _createdScans; }
288  const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & createdFeatures() const { return _createdFeatures; }
289 
290  const rtabmap::OccupancyGrid * occupancyGrid() const { return _occupancyGrid; }
291  const rtabmap::OctoMap * octomap() const { return _octomap; }
292 
293  rtabmap::ProgressDialog * progressDialog() { return _progressDialog; }
294  rtabmap::CloudViewer * cloudViewer() const { return _cloudViewer; }
295  rtabmap::LoopClosureViewer * loopClosureViewer() const { return _loopClosureViewer; }
296 
297  void setCloudViewer(rtabmap::CloudViewer * cloudViewer);
298  void setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer);
299 
300  void setNewDatabasePathOutput(const QString & newDatabasePathOutput) {_newDatabasePathOutput = newDatabasePathOutput;}
301  const QString & newDatabasePathOutput() const { return _newDatabasePathOutput; }
302 
304  virtual Camera* createCamera();
305 
306 private:
307  Ui_mainWindow * _ui;
308 
313 
314  //Dialogs
322 
323  QSet<int> _lastIds;
324  int _lastId;
325  double _firstStamp;
338  QStringList _waypoints;
340  std::vector<CameraModel> _rectCameraModels;
341 
342  QMap<int, Signature> _cachedSignatures;
344  std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
345  std::map<int, Transform> _currentGTPosesMap; // <nodeId, pose>
346  std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
347  std::map<int, int> _currentMapIds; // <nodeId, mapId>
348  std::map<int, std::string> _currentLabels; // <nodeId, label>
349  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > _cachedClouds;
351  std::set<int> _cachedEmptyClouds;
352  std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
353  std::map<int, float> _cachedWordsCount;
354  std::map<int, float> _cachedLocalizationsCount;
355 
356  std::map<int, LaserScan> _createdScans;
357 
360 
361  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;
362 
366 
367  QTimer * _oneSecondTimer;
368  QTime * _elapsedTime;
369  QTime * _logEventTime;
370 
374 
376 
379 
382  QMap<int, QString> _exportPosesFileName;
386  QMap<QString, QByteArray> _autoScreenCaptureCachedImages;
387 
388  QVector<int> _refIds;
389  QVector<int> _loopClosureIds;
390 
393 };
394 
395 }
396 
397 #endif /* RTABMAP_MainWindow_H_ */
#define RTABMAPGUI_EXP
Definition: RtabmapGuiExp.h:38
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
const std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > & createdFeatures() const
Definition: MainWindow.h:288
std::set< int > _cachedEmptyClouds
Definition: MainWindow.h:351
bool isProcessingOdometry() const
Definition: MainWindow.h:108
bool isDatabaseUpdated() const
Definition: MainWindow.h:110
DepthCalibrationDialog * _depthCalibrationDialog
Definition: MainWindow.h:320
QStringList _waypoints
Definition: MainWindow.h:338
void setNewDatabasePathOutput(const QString &newDatabasePathOutput)
Definition: MainWindow.h:300
virtual ParametersMap getCustomParameters()
Definition: MainWindow.h:303
bool exportPoses
Definition: UEvent.h:57
QString _newDatabasePathOutput
Definition: MainWindow.h:331
std::map< int, float > _cachedLocalizationsCount
Definition: MainWindow.h:354
PreferencesDialog * _preferencesDialog
Definition: MainWindow.h:315
const std::map< int, LaserScan > & createdScans() const
Definition: MainWindow.h:287
const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > & cachedClouds() const
Definition: MainWindow.h:286
rtabmap::CameraThread * _camera
Definition: MainWindow.h:310
bool _autoScreenCaptureOdomSync
Definition: MainWindow.h:383
const rtabmap::OctoMap * octomap() const
Definition: MainWindow.h:291
const std::map< int, Transform > & currentGTPosesMap() const
Definition: MainWindow.h:282
std::multimap< int, Link > _currentLinksMap
Definition: MainWindow.h:346
QString _openedDatabasePath
Definition: MainWindow.h:332
rtabmap::OdometryThread * _odomThread
Definition: MainWindow.h:311
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::LoopClosureViewer * loopClosureViewer() const
Definition: MainWindow.h:295
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
std::vector< CameraModel > _rectCameraModels
Definition: MainWindow.h:340
QString _newDatabasePath
Definition: MainWindow.h:330
QMap< int, QString > _exportPosesFileName
Definition: MainWindow.h:382
long _createdCloudsMemoryUsage
Definition: MainWindow.h:350
bool isProcessingStatistics() const
Definition: MainWindow.h:107
const State & state() const
Definition: MainWindow.h:278
std::map< int, Transform > _currentPosesMap
Definition: MainWindow.h:344
Transform _odometryCorrection
Definition: MainWindow.h:363
std::pair< int, std::pair< std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr >, pcl::IndicesPtr > > _previousCloud
Definition: MainWindow.h:352
const QMap< int, Signature > & cachedSignatures() const
Definition: MainWindow.h:280
std::map< int, LaserScan > _createdScans
Definition: MainWindow.h:356
const std::map< int, Transform > & currentPosesMap() const
Definition: MainWindow.h:281
QVector< int > _refIds
Definition: MainWindow.h:388
QSet< int > _lastIds
Definition: MainWindow.h:323
rtabmap::OctoMap * _octomap
Definition: MainWindow.h:359
const QString & newDatabasePathOutput() const
Definition: MainWindow.h:301
PdfPlotCurve * _likelihoodCurve
Definition: MainWindow.h:372
rtabmap::IMUThread * _imuThread
Definition: MainWindow.h:312
const std::map< int, std::string > & currentLabels() const
Definition: MainWindow.h:285
std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > _cachedClouds
Definition: MainWindow.h:349
const std::multimap< int, Link > & currentLinksMap() const
Definition: MainWindow.h:283
std::map< int, std::string > _currentLabels
Definition: MainWindow.h:348
rtabmap::ProgressDialog * progressDialog()
Definition: MainWindow.h:293
bool isSavedMaximized() const
Definition: MainWindow.h:105
CloudViewer * _cloudViewer
Definition: MainWindow.h:377
PdfPlotCurve * _posteriorCurve
Definition: MainWindow.h:371
RecoveryProgressState state
std::map< int, Transform > _currentGTPosesMap
Definition: MainWindow.h:345
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > _createdFeatures
Definition: MainWindow.h:361
QMap< int, Signature > _cachedSignatures
Definition: MainWindow.h:342
QMap< QString, QByteArray > _autoScreenCaptureCachedImages
Definition: MainWindow.h:386
Ui_mainWindow * ui()
Definition: MainWindow.h:277
PdfPlotCurve * _rawLikelihoodCurve
Definition: MainWindow.h:373
rtabmap::OccupancyGrid * _occupancyGrid
Definition: MainWindow.h:358
std::map< int, float > _cachedWordsCount
Definition: MainWindow.h:353
std::map< int, int > _currentMapIds
Definition: MainWindow.h:347
QString _defaultOpenDatabasePath
Definition: MainWindow.h:333
ProgressDialog * _progressDialog
Definition: MainWindow.h:375
PostProcessingDialog * _postProcessingDialog
Definition: MainWindow.h:319
AboutDialog * _aboutDialog
Definition: MainWindow.h:316
ExportBundlerDialog * _exportBundlerDialog
Definition: MainWindow.h:318
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
Ui_mainWindow * _ui
Definition: MainWindow.h:307
QString _graphSavingFileName
Definition: MainWindow.h:380
QTimer * _oneSecondTimer
Definition: MainWindow.h:367
QVector< int > _loopClosureIds
Definition: MainWindow.h:389
static int newDatabase(BtShared *pBt)
Definition: sqlite3.c:52932
ExportCloudsDialog * _exportCloudsDialog
Definition: MainWindow.h:317
const std::map< int, int > & currentMapIds() const
Definition: MainWindow.h:284
Transform _lastOdomPose
Definition: MainWindow.h:364
DataRecorder * _dataRecorder
Definition: MainWindow.h:321
LoopClosureViewer * _loopClosureViewer
Definition: MainWindow.h:378
rtabmap::CloudViewer * cloudViewer() const
Definition: MainWindow.h:294
const rtabmap::OccupancyGrid * occupancyGrid() const
Definition: MainWindow.h:290
bool _processingDownloadedMap
Definition: MainWindow.h:327
static int openDatabase(const char *zFilename, sqlite3 **ppDb, unsigned int flags, const char *zVfs)
Definition: sqlite3.c:121404


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