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/OdometryEvent.h"
00039 #include "rtabmap/core/CameraInfo.h"
00040 #include "rtabmap/gui/PreferencesDialog.h"
00041
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/PolygonMesh.h>
00045 #include <pcl/pcl_base.h>
00046 #include <pcl/TextureMesh.h>
00047
00048 namespace rtabmap {
00049 class CameraThread;
00050 class OdometryThread;
00051 class CloudViewer;
00052 class LoopClosureViewer;
00053 }
00054
00055 class QGraphicsScene;
00056 class Ui_mainWindow;
00057 class QActionGroup;
00058
00059 namespace rtabmap {
00060
00061 class LikelihoodScene;
00062 class AboutDialog;
00063 class Plot;
00064 class PdfPlotCurve;
00065 class StatsToolBox;
00066 class ProgressDialog;
00067 class TwistGridWidget;
00068 class ExportCloudsDialog;
00069 class ExportScansDialog;
00070 class PostProcessingDialog;
00071 class DataRecorder;
00072 class OctoMap;
00073
00074 class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
00075 {
00076 Q_OBJECT
00077
00078 public:
00079 enum State {
00080 kIdle,
00081 kInitializing,
00082 kInitialized,
00083 kApplicationClosing,
00084 kClosing,
00085 kStartingDetection,
00086 kDetecting,
00087 kPaused,
00088 kMonitoring,
00089 kMonitoringPaused
00090 };
00091
00092 public:
00097 MainWindow(PreferencesDialog * prefDialog = 0, QWidget * parent = 0);
00098 virtual ~MainWindow();
00099
00100 QString getWorkingDirectory() const;
00101 void setMonitoringState(bool pauseChecked = false);
00102 bool isSavedMaximized() const {return _savedMaximized;}
00103
00104 bool isProcessingStatistics() const {return _processingStatistics;}
00105 bool isProcessingOdometry() const {return _processingOdometry;}
00106
00107 public slots:
00108 void processStats(const rtabmap::Statistics & stat);
00109 void updateCacheFromDatabase(const QString & path);
00110 void openDatabase(const QString & path);
00111
00112 protected:
00113 virtual void closeEvent(QCloseEvent* event);
00114 virtual void handleEvent(UEvent* anEvent);
00115 virtual void showEvent(QShowEvent* anEvent);
00116 virtual void moveEvent(QMoveEvent* anEvent);
00117 virtual void resizeEvent(QResizeEvent* anEvent);
00118 virtual bool eventFilter(QObject *obj, QEvent *event);
00119
00120 private slots:
00121 void changeState(MainWindow::State state);
00122 void beep();
00123 void configGUIModified();
00124 void saveConfigGUI();
00125 void newDatabase();
00126 void openDatabase();
00127 bool closeDatabase();
00128 void editDatabase();
00129 void startDetection();
00130 void pauseDetection();
00131 void stopDetection();
00132 void notifyNoMoreImages();
00133 void printLoopClosureIds();
00134 void generateGraphDOT();
00135 void exportPosesRaw();
00136 void exportPosesRGBDSLAM();
00137 void exportPosesKITTI();
00138 void exportPosesTORO();
00139 void exportPosesG2O();
00140 void exportImages();
00141 void exportOctomap();
00142 void postProcessing();
00143 void deleteMemory();
00144 void openWorkingDirectory();
00145 void updateEditMenu();
00146 void selectStream();
00147 void selectOpenni();
00148 void selectFreenect();
00149 void selectOpenniCv();
00150 void selectOpenniCvAsus();
00151 void selectOpenni2();
00152 void selectFreenect2();
00153 void selectStereoDC1394();
00154 void selectStereoFlyCapture2();
00155 void selectStereoZed();
00156 void selectStereoUsb();
00157 void dumpTheMemory();
00158 void dumpThePrediction();
00159 void sendGoal();
00160 void sendWaypoints();
00161 void postGoal(const QString & goal);
00162 void cancelGoal();
00163 void label();
00164 void updateCacheFromDatabase();
00165 void downloadAllClouds();
00166 void downloadPoseGraph();
00167 void anchorCloudsToGroundTruth();
00168 void clearTheCache();
00169 void openPreferences();
00170 void openPreferencesSource();
00171 void setDefaultViews();
00172 void selectScreenCaptureFormat(bool checked);
00173 void takeScreenshot();
00174 void updateElapsedTime();
00175 void processCameraInfo(const rtabmap::CameraInfo & info);
00176 void processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored);
00177 void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
00178 void applyPrefSettings(const rtabmap::ParametersMap & parameters);
00179 void processRtabmapEventInit(int status, const QString & info);
00180 void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
00181 void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
00182 void processRtabmapLabelErrorEvent(int id, const QString & label);
00183 void processRtabmapGoalStatusEvent(int status);
00184 void changeImgRateSetting();
00185 void changeDetectionRateSetting();
00186 void changeTimeLimitSetting();
00187 void changeMappingMode();
00188 void setAspectRatio(int w, int h);
00189 void setAspectRatio16_9();
00190 void setAspectRatio16_10();
00191 void setAspectRatio4_3();
00192 void setAspectRatio240p();
00193 void setAspectRatio360p();
00194 void setAspectRatio480p();
00195 void setAspectRatio720p();
00196 void setAspectRatio1080p();
00197 void setAspectRatioCustom();
00198 void exportGridMap();
00199 void exportScans();
00200 void exportClouds();
00201 void exportBundlerFormat();
00202 void viewScans();
00203 void viewClouds();
00204 void resetOdometry();
00205 void triggerNewMap();
00206 void dataRecorder();
00207 void dataRecorderDestroyed();
00208 void updateNodeVisibility(int, bool);
00209 void updateGraphView();
00210
00211 signals:
00212 void statsReceived(const rtabmap::Statistics &);
00213 void cameraInfoReceived(const rtabmap::CameraInfo &);
00214 void odometryReceived(const rtabmap::OdometryEvent &, bool);
00215 void thresholdsChanged(int, int);
00216 void stateChanged(MainWindow::State);
00217 void rtabmapEventInitReceived(int status, const QString & info);
00218 void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
00219 void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
00220 void rtabmapLabelErrorReceived(int id, const QString & label);
00221 void rtabmapGoalStatusEventReceived(int status);
00222 void imgRateChanged(double);
00223 void detectionRateChanged(double);
00224 void timeLimitChanged(float);
00225 void mappingModeChanged(bool);
00226 void noMoreImagesReceived();
00227 void loopClosureThrChanged(float);
00228 void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
00229
00230 private:
00231 void update3DMapVisibility(bool cloudsShown, bool scansShown);
00232 void updateMapCloud(
00233 const std::map<int, Transform> & poses,
00234 const std::multimap<int, Link> & constraints,
00235 const std::map<int, int> & mapIds,
00236 const std::map<int, std::string> & labels,
00237 const std::map<int, Transform> & groundTruths,
00238 bool verboseProgress = false);
00239 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
00240 void createAndAddProjectionMap(
00241 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00242 const pcl::IndicesPtr & indices,
00243 int nodeId,
00244 const Transform & pose,
00245 bool updateOctomap = false);
00246 void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
00247 void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
00248 Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
00249 void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
00250 void setupMainLayout(bool vertical);
00251 void updateSelectSourceMenu();
00252 void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
00253 void saveFigures();
00254 void loadFigures();
00255 void exportPoses(int format);
00256 QString captureScreen(bool cacheInRAM = false);
00257
00258 private:
00259 Ui_mainWindow * _ui;
00260
00261 State _state;
00262 rtabmap::CameraThread * _camera;
00263 rtabmap::OdometryThread * _odomThread;
00264
00265
00266 PreferencesDialog * _preferencesDialog;
00267 AboutDialog * _aboutDialog;
00268 ExportCloudsDialog * _exportCloudsDialog;
00269 ExportScansDialog * _exportScansDialog;
00270 PostProcessingDialog * _postProcessingDialog;
00271 DataRecorder * _dataRecorder;
00272
00273 QSet<int> _lastIds;
00274 int _lastId;
00275 double _firstStamp;
00276 bool _processingStatistics;
00277 bool _processingDownloadedMap;
00278 bool _odometryReceived;
00279 QString _newDatabasePath;
00280 QString _newDatabasePathOutput;
00281 QString _openedDatabasePath;
00282 bool _databaseUpdated;
00283 bool _odomImageShow;
00284 bool _odomImageDepthShow;
00285 bool _savedMaximized;
00286 QStringList _waypoints;
00287 int _waypointsIndex;
00288
00289 QMap<int, Signature> _cachedSignatures;
00290 long _cachedMemoryUsage;
00291 std::map<int, Transform> _currentPosesMap;
00292 std::map<int, Transform> _currentGTPosesMap;
00293 std::multimap<int, Link> _currentLinksMap;
00294 std::map<int, int> _currentMapIds;
00295 std::map<int, std::string> _currentLabels;
00296 std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > _cachedClouds;
00297 long _createdCloudsMemoryUsage;
00298 std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud;
00299
00300 std::map<int, cv::Mat> _createdScans;
00301 std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps;
00302 std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps;
00303
00304 rtabmap::OctoMap * _octomap;
00305
00306 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;
00307
00308 Transform _odometryCorrection;
00309 Transform _lastOdomPose;
00310 bool _processingOdometry;
00311
00312 QTimer * _oneSecondTimer;
00313 QTime * _elapsedTime;
00314 QTime * _logEventTime;
00315
00316 PdfPlotCurve * _posteriorCurve;
00317 PdfPlotCurve * _likelihoodCurve;
00318 PdfPlotCurve * _rawLikelihoodCurve;
00319
00320 ProgressDialog * _initProgressDialog;
00321
00322 CloudViewer * _cloudViewer;
00323 LoopClosureViewer * _loopClosureViewer;
00324
00325 QString _graphSavingFileName;
00326 QMap<int, QString> _exportPosesFileName;
00327 bool _autoScreenCaptureOdomSync;
00328 bool _autoScreenCaptureRAM;
00329 QMap<QString, QByteArray> _autoScreenCaptureCachedImages;
00330
00331 QVector<int> _refIds;
00332 QVector<int> _loopClosureIds;
00333
00334 bool _firstCall;
00335 };
00336
00337 }
00338
00339 #endif