00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef MAINWINDOW_H_
00029 #define MAINWINDOW_H_
00030
00031 #include "rtabmap/gui/RtabmapGuiExp.h"
00032
00033 #include "rtabmap/utilite/UEventsHandler.h"
00034 #include <QMainWindow>
00035 #include <QtCore/QSet>
00036 #include "rtabmap/core/RtabmapEvent.h"
00037 #include "rtabmap/core/SensorData.h"
00038 #include "rtabmap/core/OdometryInfo.h"
00039 #include "rtabmap/gui/PreferencesDialog.h"
00040
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl/PolygonMesh.h>
00044
00045 namespace rtabmap {
00046 class CameraThread;
00047 class DBReader;
00048 class CameraOpenni;
00049 class CameraFreenect;
00050 class OdometryThread;
00051 class CloudViewer;
00052 }
00053
00054 class QGraphicsScene;
00055 class Ui_mainWindow;
00056 class QActionGroup;
00057
00058 namespace rtabmap {
00059
00060 class LikelihoodScene;
00061 class AboutDialog;
00062 class Plot;
00063 class PdfPlotCurve;
00064 class StatsToolBox;
00065 class DetailedProgressDialog;
00066 class TwistGridWidget;
00067 class ExportCloudsDialog;
00068 class PostProcessingDialog;
00069 class DataRecorder;
00070
00071 class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
00072 {
00073 Q_OBJECT
00074
00075 public:
00076 enum State {
00077 kIdle,
00078 kInitializing,
00079 kInitialized,
00080 kApplicationClosing,
00081 kClosing,
00082 kStartingDetection,
00083 kDetecting,
00084 kPaused,
00085 kMonitoring,
00086 kMonitoringPaused
00087 };
00088
00089 enum SrcType {
00090 kSrcUndefined,
00091 kSrcVideo,
00092 kSrcImages,
00093 kSrcStream
00094 };
00095
00096 public:
00101 MainWindow(PreferencesDialog * prefDialog = 0, QWidget * parent = 0);
00102 virtual ~MainWindow();
00103
00104 QString getWorkingDirectory() const;
00105 void setMonitoringState(bool pauseChecked = false);
00106 bool isSavedMaximized() const {return _savedMaximized;}
00107
00108 bool isProcessingStatistics() const {return _processingStatistics;}
00109 bool isProcessingOdometry() const {return _processingOdometry;}
00110
00111 public slots:
00112 void processStats(const rtabmap::Statistics & stat);
00113
00114 protected:
00115 virtual void closeEvent(QCloseEvent* event);
00116 virtual void handleEvent(UEvent* anEvent);
00117 virtual void showEvent(QShowEvent* anEvent);
00118 virtual void moveEvent(QMoveEvent* anEvent);
00119 virtual void resizeEvent(QResizeEvent* anEvent);
00120 virtual bool eventFilter(QObject *obj, QEvent *event);
00121
00122 private slots:
00123 void changeState(MainWindow::State state);
00124 void beep();
00125 void configGUIModified();
00126 void saveConfigGUI();
00127 void newDatabase();
00128 void openDatabase();
00129 bool closeDatabase();
00130 void editDatabase();
00131 void startDetection();
00132 void pauseDetection();
00133 void stopDetection();
00134 void printLoopClosureIds();
00135 void generateMap();
00136 void generateLocalMap();
00137 void generateTOROMap();
00138 void postProcessing();
00139 void deleteMemory();
00140 void openWorkingDirectory();
00141 void updateEditMenu();
00142 void selectImages();
00143 void selectVideo();
00144 void selectStream();
00145 void selectDatabase();
00146 void selectOpenni();
00147 void selectFreenect();
00148 void selectOpenniCv();
00149 void selectOpenniCvAsus();
00150 void selectOpenni2();
00151 void selectFreenect2();
00152 void selectStereoDC1394();
00153 void selectStereoFlyCapture2();
00154 void dumpTheMemory();
00155 void dumpThePrediction();
00156 void sendGoal();
00157 void downloadAllClouds();
00158 void downloadPoseGraph();
00159 void clearTheCache();
00160 void openPreferences();
00161 void selectScreenCaptureFormat(bool checked);
00162 void takeScreenshot();
00163 void updateElapsedTime();
00164 void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
00165 void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
00166 void applyPrefSettings(const rtabmap::ParametersMap & parameters);
00167 void processRtabmapEventInit(int status, const QString & info);
00168 void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
00169 void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
00170 void changeImgRateSetting();
00171 void changeDetectionRateSetting();
00172 void changeTimeLimitSetting();
00173 void changeMappingMode();
00174 void captureScreen();
00175 void setAspectRatio(int w, int h);
00176 void setAspectRatio16_9();
00177 void setAspectRatio16_10();
00178 void setAspectRatio4_3();
00179 void setAspectRatio240p();
00180 void setAspectRatio360p();
00181 void setAspectRatio480p();
00182 void setAspectRatio720p();
00183 void setAspectRatio1080p();
00184 void exportGridMap();
00185 void exportScans();
00186 void exportClouds();
00187 void viewScans();
00188 void viewClouds();
00189 void resetOdometry();
00190 void triggerNewMap();
00191 void dataRecorder();
00192 void dataRecorderDestroyed();
00193 void updateNodeVisibility(int, bool);
00194
00195 signals:
00196 void statsReceived(const rtabmap::Statistics &);
00197 void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &);
00198 void thresholdsChanged(int, int);
00199 void stateChanged(MainWindow::State);
00200 void rtabmapEventInitReceived(int status, const QString & info);
00201 void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
00202 void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
00203 void imgRateChanged(double);
00204 void detectionRateChanged(double);
00205 void timeLimitChanged(float);
00206 void mappingModeChanged(bool);
00207 void noMoreImagesReceived();
00208 void loopClosureThrChanged(float);
00209 void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
00210
00211 private:
00212 void update3DMapVisibility(bool cloudsShown, bool scansShown);
00213 void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints, const std::map<int, int> & mapIds, bool verboseProgress = false);
00214 void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
00215 void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
00216 void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
00217 void setupMainLayout(bool vertical);
00218 void updateSelectSourceMenu();
00219 void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
00220 void saveFigures();
00221 void loadFigures();
00222
00223 pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
00224 const std::map<int, Transform> & poses,
00225 float assembledVoxelSize,
00226 bool regenerateClouds,
00227 int regenerateDecimation,
00228 float regenerateVoxelSize,
00229 float regenerateMaxDepth) const;
00230 pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
00231 int id,
00232 const cv::Mat & rgb,
00233 const cv::Mat & depth,
00234 float fx,
00235 float fy,
00236 float cx,
00237 float cy,
00238 const Transform & localTransform,
00239 const Transform & pose,
00240 float voxelSize,
00241 int decimation,
00242 float maxDepth) const;
00243 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
00244 const std::map<int, Transform> & poses,
00245 bool regenerateClouds,
00246 int regenerateDecimation,
00247 float regenerateVoxelSize,
00248 float regenerateMaxDepth) const;
00249
00250 bool getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans);
00251 bool getExportedClouds(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool toSave);
00252 void saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode = true);
00253 void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
00254 void saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds, bool binaryMode = true);
00255
00256 private:
00257 Ui_mainWindow * _ui;
00258
00259 State _state;
00260 rtabmap::CameraThread * _camera;
00261 rtabmap::DBReader * _dbReader;
00262 rtabmap::OdometryThread * _odomThread;
00263
00264 SrcType _srcType;
00265 QString _srcPath;
00266
00267
00268 PreferencesDialog * _preferencesDialog;
00269 AboutDialog * _aboutDialog;
00270 ExportCloudsDialog * _exportDialog;
00271 PostProcessingDialog * _postProcessingDialog;
00272 DataRecorder * _dataRecorder;
00273
00274 QSet<int> _lastIds;
00275 int _lastId;
00276 bool _processingStatistics;
00277 bool _odometryReceived;
00278 QString _newDatabasePath;
00279 QString _newDatabasePathOutput;
00280 QString _openedDatabasePath;
00281 bool _databaseUpdated;
00282 bool _odomImageShow;
00283 bool _odomImageDepthShow;
00284 bool _savedMaximized;
00285
00286 QMap<int, Signature> _cachedSignatures;
00287 std::map<int, Transform> _currentPosesMap;
00288 std::multimap<int, Link> _currentLinksMap;
00289 std::map<int, int> _currentMapIds;
00290 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
00291 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
00292 std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps;
00293 std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps;
00294 Transform _odometryCorrection;
00295 Transform _lastOdomPose;
00296 bool _processingOdometry;
00297 double _lastOdomInfoUpdateTime;
00298
00299 QTimer * _oneSecondTimer;
00300 QTime * _elapsedTime;
00301 QTime * _logEventTime;
00302
00303 PdfPlotCurve * _posteriorCurve;
00304 PdfPlotCurve * _likelihoodCurve;
00305 PdfPlotCurve * _rawLikelihoodCurve;
00306
00307 DetailedProgressDialog * _initProgressDialog;
00308
00309 QString _graphSavingFileName;
00310 QString _toroSavingFileName;
00311 bool _autoScreenCaptureOdomSync;
00312
00313 QVector<int> _refIds;
00314 QVector<int> _loopClosureIds;
00315
00316 bool _firstCall;
00317 };
00318
00319 }
00320
00321 #endif