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/CloudViewer.h

@@ -0,0 +1,138 @@
+/*
+ * CloudViewer.h
+ *
+ *  Created on: 2013-10-13
+ *      Author: Mathieu
+ */
+
+#ifndef CLOUDVIEWER_H_
+#define CLOUDVIEWER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <QVTKWidget.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PolygonMesh.h>
+#include "rtabmap/core/Transform.h"
+#include <QtCore/QMap>
+
+#include <pcl/PCLPointCloud2.h>
+
+namespace pcl {
+	namespace visualization {
+		class PCLVisualizer;
+	}
+}
+
+class QMenu;
+
+namespace rtabmap {
+
+class RTABMAPGUI_EXP CloudViewer : public QVTKWidget
+{
+	Q_OBJECT
+
+public:
+	CloudViewer(QWidget * parent = 0);
+	virtual ~CloudViewer();
+
+	bool updateCloudPose(
+		const std::string & id,
+		const Transform & pose); //including mesh
+
+	bool updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose = Transform::getIdentity());
+
+	bool updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & pose = Transform::getIdentity());
+
+	bool addOrUpdateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose = Transform::getIdentity());
+
+	bool addOrUpdateCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PCLPointCloud2Ptr & binaryCloud,
+			const Transform & pose,
+			bool rgb);
+
+	bool addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose = Transform::getIdentity());
+
+	bool addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & pose = Transform::getIdentity());
+
+	bool addCloudMesh(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const Transform & pose = Transform::getIdentity());
+
+	bool addCloudMesh(
+		const std::string & id,
+		const pcl::PolygonMesh::Ptr & mesh,
+		const Transform & pose = Transform::getIdentity());
+
+	void updateCameraPosition(
+		const Transform & pose);
+
+	void setTrajectoryShown(bool shown);
+	void setTrajectorySize(int value);
+
+	void removeAllClouds(); //including meshes
+	bool removeCloud(const std::string & id); //including mesh
+
+	bool getPose(const std::string & id, Transform & pose); //including meshes
+
+	const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
+
+public slots:
+	void render();
+	void setBackgroundColor(const QColor & color);
+	void setCloudVisibility(const std::string & id, bool isVisible);
+	void setCloudOpacity(const std::string & id, double opacity = 1.0);
+	void setCloudPointSize(const std::string & id, int size);
+
+protected:
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+	virtual void handleAction(QAction * event);
+	QMenu * menu() {return _menu;}
+
+private:
+	void createMenu();
+
+private:
+    pcl::visualization::PCLVisualizer * _visualizer;
+    QAction * _aLockCamera;
+    QAction * _aFollowCamera;
+    QAction * _aResetCamera;
+    QAction * _aLockViewZ;
+    QAction * _aShowTrajectory;
+    QAction * _aSetTrajectorySize;
+    QAction * _aClearTrajectory;
+    QAction * _aShowGrid;
+    QMenu * _menu;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
+    unsigned int _maxTrajectorySize;
+    QMap<std::string, Transform> _addedClouds; // include meshes
+    Transform _lastPose;
+    std::list<std::string> _gridLines;
+};
+
+} /* namespace rtabmap */
+#endif /* CLOUDVIEWER_H_ */

guilib/include/rtabmap/gui/DataRecorder.h

@@ -0,0 +1,48 @@
+/*
+ * DataRecorder.h
+ *
+ *  Created on: 2013-10-30
+ *      Author: Mathieu
+ */
+
+#ifndef DATARECORDER_H_
+#define DATARECORDER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QtGui/QWidget>
+#include <rtabmap/core/Image.h>
+#include <rtabmap/utilite/UTimer.h>
+
+namespace rtabmap {
+
+class Memory;
+class ImageView;
+
+class RTABMAPGUI_EXP DataRecorder : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	DataRecorder(QWidget * parent = 0);
+	bool init(const QString & path);
+
+	void close();
+
+	virtual ~DataRecorder();
+
+public slots:
+	void addData(const rtabmap::Image & image);
+	void showImage(const rtabmap::Image & image);
+protected:
+	void handleEvent(UEvent * event);
+
+private:
+	Memory * memory_;
+	ImageView* imageView_;
+	UTimer timer_;
+	int dataQueue_;
+};
+
+} /* namespace rtabmap */
+#endif /* DATARECORDER_H_ */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -39,6 +39,7 @@ class QLabel;
 namespace rtabmap
 {
 class Memory;
+class ImageView;
 }
 
 class RTABMAP_EXP DatabaseViewer : public QMainWindow
@@ -54,6 +55,7 @@ private slots:
 	void openDatabase();
 	void generateGraph();
 	void generateLocalGraph();
+	void generate3DMap();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);
@@ -61,19 +63,18 @@ private slots:
 
 private:
 	void updateIds();
-	QImage ipl2QImage(const IplImage *newImage);
-	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
 	void update(int value,
 				QLabel * labelIndex,
 				QLabel * labelActions,
 				QLabel * labelParents,
 				QLabel * labelChildren,
-				QGraphicsView * view,
+				rtabmap::ImageView * view,
 				QLabel * labelId);
 
 private:
 	Ui_DatabaseViewer * ui_;
 	QMap<int, QByteArray> imagesMap_;
+	QMap<int, QByteArray> depthImagesMap_;
 	QList<int> ids_;
 	rtabmap::Memory * memory_;
 	QString pathDatabase_;

guilib/include/rtabmap/gui/ImageView.h

@@ -20,17 +20,21 @@
 #ifndef IMAGEVIEW_H_
 #define IMAGEVIEW_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <QtGui/QGraphicsView>
 #include <QtCore/QRectF>
+#include <opencv2/features2d/features2d.hpp>
+#include <map>
 
 class QAction;
 class QMenu;
 
 namespace rtabmap {
 
-class RTABMAP_EXP ImageView : public QGraphicsView {
+class KeypointItem;
+
+class RTABMAPGUI_EXP ImageView : public QGraphicsView {
 
 	Q_OBJECT
 
@@ -41,12 +45,21 @@ public:
 	void resetZoom();
 
 	bool isImageShown();
+	bool isImageDepthShown();
 	bool isFeaturesShown();
 	bool isLinesShown();
 
 	void setFeaturesShown(bool shown);
+	void setImageShown(bool shown);
+	void setImageDepthShown(bool shown);
 	void setLinesShown(bool shown);
 
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords);
+	void setImage(const QImage & image);
+	void setImageDepth(const QImage & image);
+
+	void clear();
+
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * e);
 	virtual void wheelEvent(QWheelEvent * e);
@@ -55,7 +68,7 @@ private slots:
 	void updateZoom();
 
 private:
-	void updateItemsShown();
+	void updateOpacity();
 
 private:
 	int _zoom;
@@ -64,9 +77,14 @@ private:
 
 	QMenu * _menu;
 	QAction * _showImage;
+	QAction * _showImageDepth;
 	QAction * _showFeatures;
 	QAction * _showLines;
 	QAction * _saveImage;
+
+	QList<rtabmap::KeypointItem *> _features;
+	QGraphicsPixmapItem * _image;
+	QGraphicsPixmapItem * _imageDepth;
 };
 
 }

guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -0,0 +1,60 @@
+/*
+ * LoopClosureViewer.h
+ *
+ *  Created on: 2013-10-21
+ *      Author: Mathieu
+ */
+
+#ifndef LOOPCLOSUREVIEWER_H_
+#define LOOPCLOSUREVIEWER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <opencv2/opencv.hpp>
+#include <QtGui/QWidget>
+
+class Ui_loopClosureViewer;
+
+namespace rtabmap {
+
+class Signature;
+
+class RTABMAPGUI_EXP LoopClosureViewer : public QWidget {
+
+	Q_OBJECT
+
+public:
+	LoopClosureViewer(QWidget * parent);
+	virtual ~LoopClosureViewer();
+
+	// take ownership
+	void setData(Signature * sA, Signature * sB); // sB contains loop transform as pose() from sA
+
+	const Signature * sA() const {return sA_;}
+	const Signature * sB() const {return sB_;}
+
+public slots:
+	void setDecimation(int decimation) {decimation_ = decimation;}
+	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
+	void setSamples(int samples) {samples_ = samples;}
+	void updateView(const Transform & AtoB = Transform());
+
+protected:
+	virtual void showEvent(QShowEvent * event);
+
+private:
+	Ui_loopClosureViewer * ui_;
+
+	Signature * sA_;
+	Signature * sB_;
+	Transform transform_;
+
+	int decimation_;
+	float maxDepth_;
+	int samples_;
+};
+
+} /* namespace rtabmap */
+#endif /* LOOPCLOSUREVIEWER_H_ */

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;

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -0,0 +1,52 @@
+/*
+ * OdometryViewer.h
+ *
+ *  Created on: 2013-10-15
+ *      Author: Mathieu
+ */
+
+#ifndef ODOMETRYVIEWER_H_
+#define ODOMETRYVIEWER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include "rtabmap/core/Image.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UMutex.h"
+
+namespace rtabmap {
+
+class RTABMAPGUI_EXP OdometryViewer : public CloudViewer, public UEventsHandler
+{
+	Q_OBJECT
+
+public:
+	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, QWidget * parent = 0);
+	virtual ~OdometryViewer() {}
+
+protected:
+	void handleAction(QAction * a);
+	virtual void handleEvent(UEvent * event);
+
+private slots:
+	void processData();
+
+private:
+	UMutex dataMutex_;
+	std::list<rtabmap::Image> buffer_;
+	UTimer timer_;
+	int maxClouds_;
+	float voxelSize_;
+	int decimation_;
+	int id_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
+	QAction * _aSetVoxelSize;
+	QAction * _aSetDecimation;
+	QAction * _aSetCloudHistorySize;
+	QAction * _aPause;
+};
+
+} /* namespace rtabmap */
+#endif /* ODOMETRYVIEWER_H_ */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -20,12 +20,14 @@
 #ifndef PREFERENCESDIALOG_H_
 #define PREFERENCESDIALOG_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <QtGui/QDialog>
 #include <QtCore/QModelIndex>
+#include <QtCore/QVector>
 #include <set>
 
+#include "rtabmap/core/Transform.h"
 #include "rtabmap/core/Parameters.h"
 
 class Ui_preferencesDialog;
@@ -40,22 +42,30 @@ class QLineEdit;
 class QSlider;
 class QProgressDialog;
 class UPlotCurve;
+class QStackedWidget;
+class QCheckBox;
+class QSpinBox;
+class QDoubleSpinBox;
 
 namespace rtabmap {
 
-class RTABMAP_EXP PreferencesDialog : public QDialog
+class CameraOpenni;
+class OdometryThread;
+class Signature;
+class LoopClosureViewer;
+
+class RTABMAPGUI_EXP PreferencesDialog : public QDialog
 {
 	Q_OBJECT
 
 public:
 	enum PanelFlag {
 		kPanelDummy = 0,
-		kPanelGeneralStrategy = 1,
-		kPanelGeneral = 2,
-		kPanelFourier = 4,
-		kPanelSurf = 8,
-		kPanelSource = 16,
-		kPanelAll = 31
+		kPanelGeneral = 1,
+		kPanelCloudRendering = 2,
+		kPanelLogging = 4,
+		kPanelSource = 8,
+		kPanelAll = 15
 	};
 	// TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared...
 	Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
@@ -67,11 +77,17 @@ public:
 		kSrcVideo
 	};
 
+	enum OdomTest {
+		kOdomBIN,
+		kOdomBOW,
+		kOdomICP
+	};
+
 public:
 	PreferencesDialog(QWidget * parent = 0);
 	virtual ~PreferencesDialog();
 
-	virtual QString getIniFilePath();
+	virtual QString getIniFilePath() const;
 	void init();
 
 	void saveWindowGeometry(const QString & windowName, const QWidget * window);
@@ -96,12 +112,28 @@ public:
 	bool imageHighestHypShown() const;
 	bool beepOnPause() const;
 	int getKeypointsOpacity() const;
-	QString getWorkingDirectory();
+
+	bool isCloudMeshing(int index) const;     // 0=map
+	bool isCloudsShown(int index) const;      // 0=map, 1=odom, 2=save
+	double getCloudVoxelSize(int index) const; // 0=map, 1=odom, 2=save
+	int getCloudDecimation(int index) const;   // 0=map, 1=odom, 2=save
+	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom, 2=save
+	double getCloudOpacity(int index) const;   // 0=map, 1=odom, 2=save
+	int getCloudPointSize(int index) const;    // 0=map, 1=odom, 2=save
+
+	bool isScansShown(int index) const;       // 0=map, 1=odom, 2=save
+	double getScanOpacity(int index) const;    // 0=map, 1=odom, 2=save
+	int getScanPointSize(int index) const;     // 0=map, 1=odom, 2=save
+
+	QString getWorkingDirectory() const;
 
 	// source panel
 	double getGeneralInputRate() const;
 	bool isSourceImageUsed() const;
 	bool isSourceDatabaseUsed() const;
+	bool isSourceOpenniUsed() const;
+	bool isSourceOpenniOdometryBIN() const;
+	bool isSourceOpenniOdometryBOW() const;
 	bool getGeneralAutoRestart() const;
 	bool getGeneralCameraKeypoints() const;
 	int getSourceImageType() const;
@@ -117,13 +149,18 @@ public:
 	QString getSourceVideoPath() const;	//Video group
 	int getSourceUsbDeviceId() const;		//UsbDevice group
 	QString getSourceDatabasePath() const; //Database group
+	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
+	QString getSourceOpenniDevice() const;            //Openni group
+	Transform getSourceOpenniLocalTransform() const;    //Openni group
 
 	int getIgnoredDCComponents() const;
 
 	//
 	bool isImagesKept() const;
 	float getTimeLimit() const;
+	float getDetectionRate() const;
+	bool isSLAMMode() const;
 
 	//specific
 	bool isStatisticsPublished() const;
@@ -132,7 +169,7 @@ public:
 	double getExpThr() const;
 
 	//
-	void disableGeneralCameraKeypoints();
+	void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}
 
 signals:
 	void settingsChanged(PreferencesDialog::PANEL_FLAGS);
@@ -140,11 +177,14 @@ signals:
 
 public slots:
 	void setInputRate(double value);
+	void setDetectionRate(double value);
 	void setHardThr(int value);
 	void setAutoRestart(bool value);
 	void setTimeLimit(float value);
+	void setSLAMMode(bool enabled);
 	void selectSourceImage(Src src = kSrcUndef);
 	void selectSourceDatabase(bool user = false);
+	void selectSourceOpenni(bool user = false);
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -153,22 +193,29 @@ private slots:
 	void loadConfigFrom();
 	void saveConfigTo();
 	void makeObsoleteGeneralPanel();
+	void makeObsoleteCloudRenderingPanel();
+	void makeObsoleteLoggingPanel();
 	void makeObsoleteSourcePanel();
 	void clicked(const QModelIndex &index);
 	void addParameter(int value);
+	void addParameter(bool value);
 	void addParameter(double value);
 	void addParameter(const QString & value);
 	void updatePredictionPlot();
 	void updateKpROI();
+	void changeDatabasePath();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
+	void cleanOdometryTest();
+	void testSourceOdometry();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );
+	virtual void closeEvent(QCloseEvent *event);
 
 	void setParameter(const std::string & key, const std::string & value);
 
@@ -189,12 +236,17 @@ private:
 	void setupSignals();
 	void setupKpRoiPanel();
 	bool parseModel(QList<QGroupBox*> & boxes, QStandardItem * parentItem, int currentLevel, int & absoluteIndex);
+	void resetSettings(QGroupBox * groupBox);
 	void addParameter(const QObject * object, int value);
+	void addParameter(const QObject * object, bool value);
 	void addParameter(const QObject * object, double value);
 	void addParameter(const QObject * object, const QString & value);
+	void addParameters(const QObjectList & children);
+	void addParameters(const QStackedWidget * stackedWidget);
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();
+	void testOdometry(OdomTest test);
 
 protected:
 	rtabmap::ParametersMap _parameters;
@@ -204,8 +256,24 @@ private:
 	Ui_preferencesDialog * _ui;
 	QStandardItemModel * _indexModel;
 	bool _initialized;
+	bool _monitoringState;
 
 	QProgressDialog * _progressDialog;
+
+	//Odometry test
+	CameraOpenni * _odomCamera;
+	OdometryThread * _odomThread;
+
+	QVector<QCheckBox*> _3dRenderingShowClouds;
+	QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;
+	QVector<QSpinBox*> _3dRenderingDecimation;
+	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
+	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
+	QVector<QSpinBox*> _3dRenderingPtSize;
+	QVector<QCheckBox*> _3dRenderingShowScans;
+	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
+	QVector<QSpinBox*> _3dRenderingPtSizeScan;
+	QVector<QCheckBox*> _3dRenderingMeshing;
 };
 
 Q_DECLARE_OPERATORS_FOR_FLAGS(PreferencesDialog::PANEL_FLAGS)

guilib/include/rtabmap/gui/UCv2Qt.h

@@ -0,0 +1,177 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite is distributed in the hope that it will be useful,
+*  but WITHOUT ANY WARRANTY; without even the implied warranty of
+*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+*  GNU Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UCV2QT_H_
+#define UCV2QT_H_
+
+#include <QtGui/QImage>
+#include <opencv2/core/core.hpp>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UThread.h>
+
+/**
+ * Convert a cv::Mat image to a QImage. Support 
+ * depth (float32, uint16) image and RGB/BGR 8bits images.
+ * @param image the cv::Mat image (can be 1 channel [CV_8U, CV_16U or CV_32F] or 3 channels [CV_U8])
+ * @param isBgr if 3 channels, it is BGR or RGB order.
+ * @return the QImage
+ */
+inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
+{
+	QImage qtemp;
+	if(!image.empty() && image.depth() == CV_8U)
+	{
+		if(image.channels()==3)
+		{
+			const unsigned char * data = image.data;
+			if(image.channels() == 3)
+			{
+				qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32);
+				for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize())
+				{
+					for(int x = 0; x < image.cols; ++x)
+					{
+						QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x;
+						if(isBgr)
+						{
+							*p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]);
+						}
+						else
+						{
+							*p = qRgb(data[x * image.channels()], data[x * image.channels()+1], data[x * image.channels()+2]);
+						}
+					}
+				}
+			}
+		}
+		else if(image.channels() == 1)
+		{
+			// mono grayscale
+			qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
+		}
+		else
+		{
+			printf("Wrong image format, must have 1 or 3 channels\n");
+		}
+	}
+	else if(image.depth() == CV_32F && image.channels()==1)
+    {
+		// Assume depth image (float in meters)
+		const float * data = (const float *)image.data;
+		float min=0, max=0;
+		uMinMax(data, image.rows*image.cols, min, max);
+		qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8);
+		for(int y = 0; y < image.rows; ++y, data += image.cols)
+		{
+			for(int x = 0; x < image.cols; ++x)
+			{
+				uchar * p = qtemp.scanLine (y) + x;
+				if(data[x] < min || data[x] > max || uIsNan(data[x]))
+				{
+					*p = 0;
+				}
+				else
+				{
+					*p = uchar(255.0f - ((data[x]-min)*255.0f)/(max-min));
+					if(*p == 255)
+					{
+						*p = 0;
+					}
+				}
+			}
+		}
+
+		QVector<QRgb> my_table;
+		for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+		qtemp.setColorTable(my_table);
+    }
+	else if(image.depth() == CV_16U && image.channels()==1)
+	{
+		// Assume depth image (unsigned short in mm)
+		const unsigned short * data = (const unsigned short *)image.data;
+		unsigned short min=data[0], max=data[0];
+		for(unsigned int i=1; i<image.total(); ++i)
+		{
+			if(!uIsNan(data[i]) && data[i] > 0)
+			{
+				if((uIsNan(min) && data[i] > 0) ||
+				   (data[i] > 0 && data[i]<min))
+				{
+					min = data[i];
+				}
+				if((uIsNan(max) && data[i] > 0) ||
+				   (data[i] > 0 && data[i]>max))
+				{
+					max = data[i];
+				}
+			}
+		}
+
+		qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8);
+		for(int y = 0; y < image.rows; ++y, data += image.cols)
+		{
+			for(int x = 0; x < image.cols; ++x)
+			{
+				uchar * p = qtemp.scanLine (y) + x;
+				if(data[x] < min || data[x] > max || uIsNan(data[x]) || max == min)
+				{
+					*p = 0;
+				}
+				else
+				{
+					*p = uchar(255.0f - (float(data[x]-min)/float(max-min))*255.0f);
+					if(*p == 255)
+					{
+						*p = 0;
+					}
+				}
+			}
+		}
+
+		QVector<QRgb> my_table;
+		for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+		qtemp.setColorTable(my_table);
+	}
+	else if(!image.empty() && image.depth() != CV_8U)
+	{
+		printf("Wrong image format, must be 8_bits/3channels or (depth) 32bitsFloat/1channel, 16bits/1channel\n");
+	}
+	return qtemp;
+}
+
+class UCvMat2QImageThread : public UThread
+{
+public:
+	UCvMat2QImageThread(const cv::Mat & image, bool isBgr = true) :
+		image_(image),
+		isBgr_(isBgr) {}
+	QImage & getQImage() {return qtImage_;}
+protected:
+	virtual void mainLoop()
+	{
+		qtImage_ = uCvMat2QImage(image_, isBgr_);
+		this->kill();
+	}
+private:
+	cv::Mat image_;
+	bool isBgr_;
+	QImage qtImage_;
+};
+
+#endif /* UCV2QT_H_ */