Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

guilib/include/rtabmap/gui/MainWindow.h

@@ -26,11 +26,19 @@
 #include <QtGui/QMainWindow>
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/Image.h"
 #include "rtabmap/gui/PreferencesDialog.h"
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+
 namespace rtabmap {
 class CameraThread;
 class DBReader;
+class CameraOpenni;
+class OdometryThread;
+class CloudViewer;
 }
 
 class QGraphicsScene;
@@ -57,7 +65,8 @@ public:
 		kStartingDetection,
 		kDetecting,
 		kPaused,
-		kMonitoring
+		kMonitoring,
+		kMonitoringPaused
 	};
 
 	enum SrcType {
@@ -76,9 +85,10 @@ public:
 	virtual ~MainWindow();
 
 	QString getWorkingDirectory() const;
+	void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
 
 public slots:
-	void changeState(MainWindow::State state);
+	void processStats(const rtabmap::Statistics & stat);
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);
@@ -86,6 +96,7 @@ protected:
 	virtual void resizeEvent(QResizeEvent* anEvent);
 
 private slots:
+	void changeState(MainWindow::State state);
 	void beep();
 	void startDetection();
 	void pauseDetection();
@@ -100,21 +111,27 @@ private slots:
 	void selectVideo();
 	void selectStream();
 	void selectDatabase();
+	void selectOpenni();
 	void resetTheMemory();
 	void dumpTheMemory();
 	void dumpThePrediction();
+	void downloadAllClouds();
 	void clearTheCache();
 	void saveFigures();
 	void loadFigures();
+	void openPreferences();
 	void selectScreenCaptureFormat(bool checked);
 	void updateElapsedTime();
-	void processStats(const rtabmap::Statistics & stat);
+	void processOdometry(const rtabmap::Image & data);
 	void applyAllPrefSettings();
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
 	void processRtabmapEventInit(int status, const QString & info);
+	void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
 	void changeImgRateSetting();
+	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
+	void changeMappingMode();
 	void captureScreen();
 	void setAspectRatio(int w, int h);
 	void setAspectRatio16_9();
@@ -125,23 +142,53 @@ private slots:
 	void setAspectRatio480p();
 	void setAspectRatio720p();
 	void setAspectRatio1080p();
+	void savePointClouds();
+	void saveMeshes();
+	void viewPointClouds();
+	void viewMeshes();
+	void resetOdometry();
+	void triggerNewMap();
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
+	void odometryReceived(const rtabmap::Image &);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);
+	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
 	void imgRateChanged(double);
+	void detectionRateChanged(double);
 	void timeLimitChanged(float);
+	void mappingModeChanged(bool);
 	void noMoreImagesReceived();
 	void loopClosureThrChanged(float);
 	void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
 
 private:
+	void update3DMapVisibility(bool cloudsShown, bool scansShown);
+	void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceImageMenu(int type);
 	void updateSelectSourceDatabase(bool used);
+	void updateSelectSourceOpenni(bool used);
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			int id,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			float depthConstant,
+			const Transform & localTransform,
+			const Transform & pose,
+			float voxelSize,
+			int decimation,
+			float maxDepth);
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > createPointClouds();
+	std::map<int, pcl::PolygonMesh::Ptr> createMeshes();
+
+	void savePointClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
+	void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes);
 
 private:
 	Ui_mainWindow * _ui;
@@ -149,6 +196,8 @@ private:
 	State _state;
 	rtabmap::CameraThread * _camera;
 	rtabmap::DBReader * _dbReader;
+	rtabmap::CameraOpenni * _cameraOpenni;
+	rtabmap::OdometryThread * _odomThread;
 
 	SrcType _srcType;
 	QString _srcPath;
@@ -160,8 +209,17 @@ private:
 	QSet<int> _lastIds;
 	int _lastId;
 	bool _processingStatistics;
+	bool _odometryReceived;
 
-	QMap<int, QByteArray> _imagesMap;
+	QMap<int, std::vector<unsigned char> > _imagesMap;
+	QMap<int, std::vector<unsigned char> > _depthsMap;
+	QMap<int, std::vector<unsigned char> > _depths2DMap;
+	QMap<int, float> _depthConstantsMap;
+	QMap<int, Transform> _localTransformsMap;
+	std::map<int, Transform> _currentPosesMap;
+	Transform _odometryCorrection;
+	Transform _lastOdomPose;
+	bool _lastOdometryProcessed;
 
 	QTimer * _oneSecondTimer;
 	QTime * _elapsedTime;
@@ -172,7 +230,6 @@ private:
 	PdfPlotCurve * _rawLikelihoodCurve;
 
 	DetailedProgressDialog * _initProgressDialog;
-	QActionGroup * _selectSourceImageGrp;
 
 	QString _graphSavingFileName;
 	QString _autoScreenCaptureFormat;