feat(slam): add rtabmap_ros

rtabmap/guilib/CMakeLists.txt

@@ -0,0 +1 @@
+ADD_SUBDIRECTORY( src )

rtabmap/guilib/include/rtabmap/gui/AboutDialog.h

@@ -0,0 +1,55 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_ABOUTDIALOG_H_
+#define RTABMAP_ABOUTDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QtCore/QUrl>
+
+class Ui_aboutDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT AboutDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	AboutDialog(QWidget * parent = 0);
+
+	virtual ~AboutDialog();
+
+private:
+	Ui_aboutDialog * _ui;
+};
+
+}
+
+#endif /* ABOUTDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -0,0 +1,171 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CALIBRATIONDIALOG_H_
+#define RTABMAP_CALIBRATIONDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QSettings>
+#include <opencv2/opencv.hpp>
+
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
+
+#include <rtabmap/utilite/UEventsHandler.h>
+
+#if defined(HAVE_OPENCV_OBJDETECT) && (CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7))
+#include <opencv2/objdetect/charuco_detector.hpp>
+#define HAVE_CHARUCO
+#elif defined(HAVE_OPENCV_ARUCO)
+#include <opencv2/aruco/charuco.hpp>
+#define HAVE_CHARUCO
+#endif
+
+class Ui_calibrationDialog;
+class QTextStream;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT CalibrationDialog  : public QDialog, public UEventsHandler
+{
+	Q_OBJECT;
+
+public:
+	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0);
+	virtual ~CalibrationDialog();
+
+	bool isCalibrated() const {return models_[0].isValidForProjection() && (stereo_?models_[1].isValidForProjection():true);}
+	const rtabmap::CameraModel & getLeftCameraModel() const {return models_[0];}
+	const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];}
+	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
+	bool isProcessing() const {return processingData_;}
+	int getStereoPairs() const {return (int)stereoImagePoints_[0].size();}
+	int boardWidth() const;
+	int boardHeight() const;
+	double squareSize() const;
+	double markerLength() const;
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+	void resetSettings();
+
+	void setCameraName(const QString & name);
+	void setProgressVisibility(bool visible);
+	void setSwitchedImages(bool switched);
+	void setFisheyeModel();
+	void setPlumbobModel();
+	void setRationalModel();
+	void setStereoMode(bool stereo, const QString & leftSuffix = "left", const QString & rightSuffix = "right");
+	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
+
+	StereoCameraModel stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification, QTextStream * logStream = 0) const;
+
+public Q_SLOTS:
+
+	void setBoardType(int type);
+	void setBoardWidth(int width);
+	void setBoardHeight(int height);
+	void setSquareSize(double size);
+	void setMarkerDictionary(int dictionary);
+	void setMarkerLength(double length);
+	void setSubpixelRefinement(bool enabled);
+	void setSubpixelMaxError(double value);
+	void setCalibrationDataSaved(bool enabled);
+	void setExpectedStereoBaseline(double length);
+	void setMaxScale(int scale);
+
+	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
+	void generateBoard();
+	void calibrate();
+	void restart();
+	bool save();
+
+private Q_SLOTS:
+	void unlock();
+
+protected:
+	virtual void closeEvent(QCloseEvent* event);
+	virtual bool handleEvent(UEvent * event);
+
+private:
+	float getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
+	float getSkew(const std::vector<cv::Point2f> & fourCorners);
+	float getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
+
+	// x -> [0, 1] (left, right)
+	// y -> [0, 1] (top, bottom)
+	// size -> [0, 1] (small -> big)
+	// skew -> [0, 1] (low, high)
+	void getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
+			float & x, float & y, float & size, float & skew);
+
+private:
+	// parameters
+	bool stereo_;
+	QString leftSuffix_;
+	QString rightSuffix_;
+	QString savingDirectory_;
+
+	QString cameraName_;
+	bool processingData_;
+	bool savedCalibration_;
+	int currentId_;
+	QString timestamp_;
+
+	std::vector<cv::Point3f> chessboardPoints_;
+	std::vector<int> chessboardPointIds_;
+#ifdef HAVE_CHARUCO
+	cv::Ptr<cv::aruco::Dictionary> markerDictionary_;
+	cv::Ptr<cv::aruco::DetectorParameters> arucoDetectorParams_;
+    cv::Ptr<cv::aruco::CharucoBoard> charucoBoard_;
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+	cv::Ptr<cv::aruco::ArucoDetector> arucoDetector_;
+	cv::Ptr<cv::aruco::CharucoDetector> charucoDetector_;
+#endif
+#endif
+
+
+	std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
+	std::vector<std::vector<std::vector<cv::Point3f> > > objectPoints_;
+	std::vector<std::vector<std::vector<float> > > imageParams_;
+	std::vector<std::vector<int > > imageIds_;
+	std::vector<std::vector<std::vector<cv::Point2f> > > stereoImagePoints_;
+	std::vector<std::vector<cv::Point3f> > stereoObjectPoints_;
+	std::vector<int> stereoImageIds_;
+	std::vector<cv::Size > imageSize_;
+	std::vector<rtabmap::CameraModel> models_;
+	rtabmap::StereoCameraModel stereoModel_;
+	std::vector<unsigned short> minIrs_;
+	std::vector<unsigned short> maxIrs_;
+
+	Ui_calibrationDialog * ui_;
+};
+
+} /* namespace rtabmap */
+#endif /* CALIBRATIONDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/CameraViewer.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CAMERAVIEWER_H_
+#define RTABMAP_CAMERAVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QDialog>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+class QSpinBox;
+class QCheckBox;
+class QPushButton;
+class QLabel;
+
+namespace rtabmap {
+
+class ImageView;
+class CloudViewer;
+class MarkerDetector;
+
+class RTABMAP_GUI_EXPORT CameraViewer : public QDialog, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	CameraViewer(
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
+	virtual ~CameraViewer();
+	void setDecimation(int value);
+
+public Q_SLOTS:
+	void showImage(const rtabmap::SensorData & data);
+protected:
+	virtual bool handleEvent(UEvent * event);
+
+private:
+	ImageView* imageView_;
+	CloudViewer* cloudView_;
+	bool processingImages_;
+	QSpinBox * decimationSpin_;
+	ParametersMap parameters_;
+	QPushButton * pause_;
+	QLabel * imageSizeLabel_;
+	QCheckBox * showCloudCheckbox_;
+	QCheckBox * showScanCheckbox_;
+	QCheckBox * markerCheckbox_;
+	MarkerDetector * markerDetector_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/CloudViewer.h

@@ -0,0 +1,501 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CLOUDVIEWER_H_
+#define RTABMAP_CLOUDVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Transform.h"
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"
+#include <vtkVersionMacros.h>
+
+#if VTK_MAJOR_VERSION > 8
+#ifndef slots
+#define slots Q_SLOTS
+#endif
+#include <QVTKOpenGLNativeWidget.h>
+using PCLQVTKWidget = QVTKOpenGLNativeWidget;
+#else
+#include <QVTKWidget.h>
+using PCLQVTKWidget = QVTKWidget;
+#endif
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/TextureMesh.h>
+
+#include <QtCore/QMap>
+#include <QtCore/QSet>
+#include <QtCore/qnamespace.h>
+#include <QtCore/QSettings>
+
+#include <opencv2/opencv.hpp>
+#include <set>
+
+#include <pcl/PCLPointCloud2.h>
+
+namespace pcl {
+	namespace visualization {
+		class PCLVisualizer;
+	}
+}
+
+class QMenu;
+class vtkProp;
+template <typename T> class vtkSmartPointer;
+class vtkOBBTree;
+
+namespace rtabmap {
+
+class OctoMap;
+
+class RTABMAP_GUI_EXPORT CloudViewer : public PCLQVTKWidget
+{
+	Q_OBJECT
+
+public:
+	CloudViewer(QWidget * parent = 0, CloudViewerInteractorStyle* style = CloudViewerInteractorStyle::New());
+	virtual ~CloudViewer();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void refreshView();
+
+	bool updateCloudPose(
+		const std::string & id,
+		const Transform & pose); //including mesh
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PCLPointCloud2Ptr & binaryCloud,
+			const Transform & pose,
+			bool rgb,
+			bool hasNormals,
+			bool hasIntensity,
+			const QColor & color = QColor(),
+			int viewport = 1);
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloud(
+				const std::string & id,
+				const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+				const Transform & pose = Transform::getIdentity(),
+				const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloudMesh(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+			const std::vector<pcl::Vertices> & polygons,
+			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::PointCloud<pcl::PointXYZRGBNormal>::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());
+
+	// Only one texture per mesh is supported!
+	bool addCloudTextureMesh(
+			const std::string & id,
+			const pcl::TextureMesh::Ptr & textureMesh,
+			const cv::Mat & texture,
+			const Transform & pose = Transform::getIdentity());
+
+	bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0, bool volumeRepresentation = true);
+	void removeOctomap();
+
+	// Only one texture per mesh is supported!
+	bool addTextureMesh (
+		   const pcl::TextureMesh &mesh,
+		   const cv::Mat & texture, 
+		   const std::string &id = "texture",
+		   int viewport = 0);
+	bool addOccupancyGridMap(
+			const cv::Mat & map8U,
+			float resolution, // cell size
+			float xMin,
+			float yMin,
+			float opacity);
+	void removeOccupancyGridMap();
+
+	bool addElevationMap(
+			const cv::Mat & map32FC1,
+			float resolution, // cell size
+			float xMin,
+			float yMin,
+			float opacity);
+	void removeElevationMap();
+
+	void updateCameraTargetPosition(
+		const Transform & pose);
+
+	void updateCameraFrustum(
+			const Transform & pose,
+			const StereoCameraModel & model);
+
+	void updateCameraFrustum(
+			const Transform & pose,
+			const CameraModel & model);
+
+	void updateCameraFrustums(
+			const Transform & pose,
+			const std::vector<CameraModel> & models);
+
+	void updateCameraFrustums(
+			const Transform & pose,
+			const std::vector<StereoCameraModel> & models);
+
+	void addOrUpdateCoordinate(
+			const std::string & id,
+			const Transform & transform,
+			double scale,
+			bool foreground=false);
+	bool updateCoordinatePose(
+			const std::string & id,
+			const Transform & transform);
+	void removeCoordinate(const std::string & id);
+	void removeAllCoordinates(const std::string & prefix = "");
+	const std::set<std::string> & getAddedCoordinates() const {return _coordinates;}
+
+	void addOrUpdateLine(
+				const std::string & id,
+				const Transform & from,
+				const Transform & to,
+				const QColor & color,
+				bool arrow = false,
+				bool foreground = false);
+	void removeLine(const std::string & id);
+	void removeAllLines();
+	const std::set<std::string> & getAddedLines() const {return _lines;}
+
+	void addOrUpdateSphere(
+				const std::string & id,
+				const Transform & pose,
+				float radius,
+				const QColor & color,
+				bool foreground = false);
+	void removeSphere(const std::string & id);
+	void removeAllSpheres();
+	const std::set<std::string> & getAddedSpheres() const {return _spheres;}
+
+	void addOrUpdateCube(
+				const std::string & id,
+				const Transform & pose, // center of the cube
+				float width,  // e.g., along x axis
+				float height, // e.g., along y axis
+				float depth,  // e.g., along z axis
+				const QColor & color,
+				bool wireframe = false,
+				bool foreground = false);
+	void removeCube(const std::string & id);
+	void removeAllCubes();
+	const std::set<std::string> & getAddedCubes() const {return _cubes;}
+
+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float width,
+			float height,
+			const QColor & color,
+			bool foreground = false);
+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float widthLeft,
+			float widthRight,
+			float heightBottom,
+			float heightTop,
+			const QColor & color,
+			bool foreground = false);
+	void removeQuad(const std::string & id);
+	void removeAllQuads();
+	const std::set<std::string> & getAddedQuads() const {return _quads;}
+
+	void addOrUpdateFrustum(
+			const std::string & id,
+			const Transform & pose,
+			const Transform & localTransform,
+			double scale,
+			const QColor & color = QColor(),
+			float fovX=1.1,
+			float fovY=0.85);
+	bool updateFrustumPose(
+			const std::string & id,
+			const Transform & pose);
+	void removeFrustum(const std::string & id);
+	void removeAllFrustums(bool exceptCameraReference = false);
+	const QMap<std::string, Transform> & getAddedFrustums() const {return _frustums;}
+
+	void addOrUpdateGraph(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
+			const QColor & color = Qt::gray);
+	void removeGraph(const std::string & id);
+	void removeAllGraphs();
+
+	void addOrUpdateText(
+			const std::string & id,
+			const std::string & text,
+			const Transform & position,
+			double scale,
+			const QColor & color,
+			bool foreground = true);
+	void removeText(const std::string & id);
+	void removeAllTexts();
+	const std::set<std::string> & getAddedTexts() const {return _texts;}
+
+	bool isTrajectoryShown() const;
+	unsigned int getTrajectorySize() const;
+	void setTrajectoryShown(bool shown);
+	void setTrajectorySize(unsigned int value);
+	void clearTrajectory();
+	bool isCameraAxisShown() const;
+	void setCameraAxisShown(bool shown);
+	double getCoordinateFrameScale() const;
+	void setCoordinateFrameScale(double scale);
+	bool isFrustumShown() const;
+	float getFrustumScale() const;
+	QColor getFrustumColor() const;
+	void setFrustumShown(bool shown);
+	void setFrustumScale(float value);
+	void setFrustumColor(QColor value);
+	void resetCamera();
+
+	void removeAllClouds(); //including meshes
+	bool removeCloud(const std::string & id); //including mesh
+
+	bool getPose(const std::string & id, Transform & pose); //including meshes
+	bool getCloudVisibility(const std::string & id);
+
+	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
+	const QColor & getDefaultBackgroundColor() const;
+	const QColor & getBackgroundColor() const;
+	Transform getTargetPose() const;
+	std::string getIdByActor(vtkProp * actor) const;
+	QColor getColor(const std::string & id);
+	void setColor(const std::string & id, const QColor & color);
+
+	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
+	void setPolygonPicking(bool enabled);
+	void setRenderingRate(double rate);
+	void setEDLShading(bool on);
+	void setLighting(bool on);
+	void setShading(bool on);
+	void setEdgeVisibility(bool visible);
+	void setScalarVisibility(bool visible);
+	void setInteractorLayer(int layer);
+
+	bool isBackfaceCulling() const;
+	bool isFrontfaceCulling() const;
+	bool isPolygonPicking() const;
+	bool isEDLShadingOn() const;
+	bool isLightingOn() const;
+	bool isShadingOn() const;
+	bool isEdgeVisible() const;
+	double getRenderingRate() const;
+
+	void getCameraPosition(
+			float & x, float & y, float & z,
+			float & focalX, float & focalY, float & focalZ,
+			float & upX, float & upY, float & upZ) const;
+	bool isCameraTargetLocked() const;
+	bool isCameraTargetFollow() const;
+	bool isCameraFree() const;
+	bool isCameraLockZ() const;
+	bool isCameraOrtho() const;
+	bool isGridShown() const;
+	unsigned int getGridCellCount() const;
+	float getGridCellSize() const;
+
+	void setCameraPosition(
+			float x, float y, float z,
+			float focalX, float focalY, float focalZ,
+			float upX, float upY, float upZ);
+	void setCameraTargetLocked(bool enabled = true);
+	void setCameraTargetFollow(bool enabled = true);
+	void setCameraFree();
+	void setCameraLockZ(bool enabled = true);
+	void setCameraOrtho(bool enabled = true);
+	void setGridShown(bool shown);
+	void setNormalsShown(bool shown);
+	void setGridCellCount(unsigned int count);
+	void setGridCellSize(float size);
+	bool isNormalsShown() const;
+	int getNormalsStep() const;
+	float getNormalsScale() const;
+	void setNormalsStep(int step);
+	void setNormalsScale(float scale);
+	bool isIntensityRedColormap() const;
+	bool isIntensityRainbowColormap() const;
+	float getIntensityMax() const;
+	void setIntensityRedColormap(bool value);
+	void setIntensityRainbowColormap(bool value);
+	void setIntensityMax(float value);
+	void buildPickingLocator(bool enable);
+	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
+
+public Q_SLOTS:
+	void setDefaultBackgroundColor(const QColor & color);
+	void setBackgroundColor(const QColor & color);
+	void setCloudVisibility(const std::string & id, bool isVisible);
+	void setCloudColorIndex(const std::string & id, int index);
+	void setCloudOpacity(const std::string & id, double opacity = 1.0);
+	void setCloudPointSize(const std::string & id, int size);
+	virtual void clear();
+
+Q_SIGNALS:
+	void configChanged();
+
+protected:
+	virtual void keyReleaseEvent(QKeyEvent * event);
+	virtual void keyPressEvent(QKeyEvent * event);
+	virtual void mousePressEvent(QMouseEvent * event);
+	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void wheelEvent(QWheelEvent * event);
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+	virtual void handleAction(QAction * event);
+	QMenu * menu() {return _menu;}
+	pcl::visualization::PCLVisualizer * visualizer() {return _visualizer;}
+
+private:
+	void createMenu();
+	void addGrid();
+	void removeGrid();
+
+private:
+    pcl::visualization::PCLVisualizer * _visualizer;
+    QAction * _aLockCamera;
+    QAction * _aFollowCamera;
+    QAction * _aResetCamera;
+    QAction * _aLockViewZ;
+    QAction * _aCameraOrtho;
+    QAction * _aShowTrajectory;
+    QAction * _aSetTrajectorySize;
+    QAction * _aClearTrajectory;
+    QAction * _aShowCameraAxis;
+    QAction * _aSetFrameScale;
+    QAction * _aShowFrustum;
+    QAction * _aSetFrustumScale;
+    QAction * _aSetFrustumColor;
+    QAction * _aShowGrid;
+    QAction * _aSetGridCellCount;
+    QAction * _aSetGridCellSize;
+    QAction * _aShowNormals;
+	QAction * _aSetNormalsStep;
+	QAction * _aSetNormalsScale;
+	QAction * _aSetIntensityRedColormap;
+	QAction * _aSetIntensityRainbowColormap;
+	QAction * _aSetIntensityMaximum;
+    QAction * _aSetBackgroundColor;
+    QAction * _aSetRenderingRate;
+    QAction * _aSetEDLShading;
+    QAction * _aSetLighting;
+    QAction * _aSetFlatShading;
+    QAction * _aSetEdgeVisibility;
+	QAction * _aSetScalarVisibility;
+    QAction * _aBackfaceCulling;
+    QAction * _aPolygonPicking;
+    QMenu * _menu;
+    std::set<std::string> _graphes;
+    std::set<std::string> _coordinates;
+    std::set<std::string> _texts;
+    std::set<std::string> _lines;
+    std::set<std::string> _spheres;
+    std::set<std::string> _cubes;
+    std::set<std::string> _quads;
+    QMap<std::string, Transform> _frustums;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
+    unsigned int _maxTrajectorySize;
+    float _frustumScale;
+    QColor _frustumColor;
+    unsigned int _gridCellCount;
+    float _gridCellSize;
+    int _normalsStep;
+    float _normalsScale;
+    bool _buildLocator;
+    std::map<std::string, vtkSmartPointer<vtkOBBTree> > _locators;
+    cv::Vec3d _lastCameraOrientation;
+    cv::Vec3d _lastCameraPose;
+    QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
+    Transform _lastPose;
+    std::list<std::string> _gridLines;
+    QSet<Qt::Key> _keysPressed;
+    QColor _defaultBgColor;
+    QColor _currentBgColor;
+    bool _frontfaceCulling;
+    double _renderingRate;
+    vtkProp * _octomapActor;
+    float _intensityAbsMax;
+    double  _coordinateFrameScale;
+};
+
+} /* namespace rtabmap */
+#endif /* CLOUDVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/CloudViewerCellPicker.h

@@ -0,0 +1,41 @@
+/*
+ * CloudViewerCellPicker.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+#define GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <vtkCellPicker.h>
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT CloudViewerCellPicker : public vtkCellPicker {
+public:
+public:
+    static CloudViewerCellPicker *New ();
+    vtkTypeMacro(CloudViewerCellPicker, vtkCellPicker);
+	CloudViewerCellPicker();
+	virtual ~CloudViewerCellPicker();
+
+protected:
+	// overrided to ignore back faces
+    virtual double IntersectActorWithLine(const double p1[3],
+    		const double p2[3],
+			double t1, double t2,
+			double tol,
+			vtkProp3D *prop,
+			vtkMapper *mapper);
+
+private:
+	vtkGenericCell * cell_; //used to accelerate picking
+	vtkIdList * pointIds_; // used to accelerate picking
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_ */

rtabmap/guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

@@ -0,0 +1,53 @@
+/*
+ * CloudViewerInteractorStyl.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+#define GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <pcl/visualization/mouse_event.h>
+#include <pcl/visualization/point_picking_event.h>
+#include <pcl/visualization/interactor_style.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+class CloudViewer;
+
+class RTABMAP_GUI_EXPORT CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
+{
+public:
+    static CloudViewerInteractorStyle *New ();
+    vtkTypeMacro(CloudViewerInteractorStyle, pcl::visualization::PCLVisualizerInteractorStyle);
+
+public:
+	CloudViewerInteractorStyle();
+	virtual void Rotate();
+	void setOrthoMode(bool enabled);
+protected:
+	virtual void OnMouseMove();
+	virtual void OnLeftButtonDown();
+	virtual void OnRightButtonDown();
+
+protected:
+	friend class CloudViewer;
+	void setCloudViewer(CloudViewer * cloudViewer) {viewer_ = cloudViewer;}
+	CloudViewer * viewer_;
+
+private:
+	unsigned int NumberOfClicks;
+	int PreviousPosition[2];
+	int ResetPixelDistance;
+	float PreviousMeasure[3];
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
+	bool orthoMode_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_ */

rtabmap/guilib/include/rtabmap/gui/ConsoleWidget.h

@@ -0,0 +1,80 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CONSOLEWIDGET_H_
+#define RTABMAP_CONSOLEWIDGET_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QWidget>
+#include <QtCore/QMutex>
+#include <QtCore/QTimer>
+#include <QtCore/QTime>
+#include <QtCore/QElapsedTimer>
+
+class Ui_consoleWidget;
+class QMessageBox;
+class QTextCursor;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT ConsoleWidget : public QWidget, public UEventsHandler
+{
+	Q_OBJECT;
+
+public:
+	ConsoleWidget(QWidget * parent = 0);
+	virtual ~ConsoleWidget();
+
+public Q_SLOTS:
+	void appendMsg(const QString & msg, int level = 1);
+
+Q_SIGNALS:
+	void msgReceived(const QString &, int);
+
+private Q_SLOTS:
+	void flushConsole();
+	void updateTextEditBufferSize();
+
+protected:
+	virtual bool handleEvent(UEvent * anEvent);
+
+private:
+	Ui_consoleWidget * _ui;
+	QMessageBox * _errorMessage;
+	QMutex _errorMessageMutex;
+	QMutex _msgListMutex;
+	QTimer _timer;
+	QElapsedTimer _time;
+	QTextCursor * _textCursor;
+	QList<QPair<QString, int> > _msgList;
+};
+
+}
+
+#endif /* CONSOLEWIDGET_H_ */

rtabmap/guilib/include/rtabmap/gui/CreateSimpleCalibrationDialog.h

@@ -0,0 +1,68 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CREATESIMPLECALIBRATIONDIALOG_H_
+#define RTABMAP_CREATESIMPLECALIBRATIONDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QSettings>
+
+class Ui_createSimpleCalibrationDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT CreateSimpleCalibrationDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	CreateSimpleCalibrationDialog(
+			const QString & savingFolder = ".",
+			const QString & cameraName = "",
+			QWidget * parent = 0);
+
+	virtual ~CreateSimpleCalibrationDialog();
+
+	void setCameraInfoDir(const QString & folder) {savingFolder_ = folder;}
+	const QString & cameraName() const {return cameraName_;}
+
+private Q_SLOTS:
+	void updateStereoView();
+	void updateSaveStatus();
+	void saveCalibration();
+
+private:
+	Ui_createSimpleCalibrationDialog * ui_;
+	QString savingFolder_;
+	QString cameraName_;
+};
+
+}
+
+#endif /* CREATESIMPLECALIBRATIONDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/DataRecorder.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_DATARECORDER_H_
+#define RTABMAP_DATARECORDER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QWidget>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMutex.h>
+
+class QLabel;
+
+namespace rtabmap {
+
+class Memory;
+class ImageView;
+
+class RTABMAP_GUI_EXPORT DataRecorder : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	DataRecorder(QWidget * parent = 0);
+	bool init(const QString & path, bool recordInRAM = true);
+
+	void closeRecorder();
+
+	virtual ~DataRecorder();
+
+	const QString & path() const {return path_;}
+
+public Q_SLOTS:
+	void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1));
+	void showImage(const cv::Mat & image, const cv::Mat & depth);
+protected:
+	virtual void closeEvent(QCloseEvent* event);
+	bool handleEvent(UEvent * event);
+
+private:
+	UMutex memoryMutex_;
+	Memory * memory_;
+	ImageView* imageView_;
+	QLabel* label_;
+	UTimer timer_;
+	QString path_;
+	bool processingImages_;
+	int count_;
+	int totalSizeKB_;
+};
+
+} /* namespace rtabmap */
+#endif /* DATARECORDER_H_ */

rtabmap/guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -0,0 +1,264 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_DATABASEVIEWER_H_
+#define RTABMAP_DATABASEVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QMainWindow>
+#include <QtCore/QByteArray>
+#include <QtCore/QMap>
+#include <QtCore/QSet>
+#include <QtGui/QImage>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <set>
+#include <vector>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/GlobalMap.h>
+
+class Ui_DatabaseViewer;
+class QGraphicsScene;
+class QGraphicsView;
+class QLabel;
+class QToolButton;
+class QDialog;
+class QSpinBox;
+
+namespace rtabmap
+{
+class DBDriver;
+class ImageView;
+class SensorData;
+class CloudViewer;
+class OctoMap;
+class ExportCloudsDialog;
+class EditDepthArea;
+class EditMapArea;
+class LinkRefiningDialog;
+class Registration;
+class RegistrationIcp;
+
+class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
+{
+	Q_OBJECT
+
+public:
+	DatabaseViewer(const QString & ini = QString(), QWidget * parent = 0);
+	virtual ~DatabaseViewer();
+	bool openDatabase(const QString & path, const ParametersMap & overridenParameters = ParametersMap());
+	bool isSavedMaximized() const {return savedMaximized_;}
+	void showCloseButton(bool visible = true);
+
+protected:
+	virtual void showEvent(QShowEvent* anEvent);
+	virtual void moveEvent(QMoveEvent* anEvent);
+	virtual void resizeEvent(QResizeEvent* anEvent);
+	virtual void keyPressEvent(QKeyEvent *event);
+	virtual void closeEvent(QCloseEvent* event);
+	virtual bool eventFilter(QObject *obj, QEvent *event);
+
+private Q_SLOTS:
+	void writeSettings();
+	void restoreDefaultSettings();
+	void configModified();
+	void openDatabase();
+	bool closeDatabase();
+	void recoverDatabase();
+	void updateInfo();
+	void updateStatistics();
+	void selectObstacleColor();
+	void selectGroundColor();
+	void selectEmptyColor();
+	void selectFrontierColor();
+	void editDepthImage();
+	void generateGraph();
+	void editSaved2DMap();
+	void exportSaved2DMap();
+	void import2DMap();
+	void regenerateSavedMap();
+	void viewOptimizedMesh();
+	void exportOptimizedMesh();
+	void updateOptimizedMesh();
+	void exportDatabase();
+	void extractImages();
+	void exportPosesRaw();
+	void exportPosesRGBDSLAMMotionCapture();
+	void exportPosesRGBDSLAM();
+	void exportPosesRGBDSLAMID();
+	void exportPosesKITTI();
+	void exportPosesTORO();
+	void exportPosesG2O();
+	void exportPosesKML();
+	void exportGPS_TXT();
+	void exportGPS_KML();
+	void generateLocalGraph();
+	void regenerateLocalMaps();
+	void regenerateCurrentLocalMaps();
+	void view3DMap();
+	void generate3DMap();
+	void detectMoreLoopClosures();
+	void updateAllNeighborCovariances();
+	void updateAllLoopClosureCovariances();
+	void updateAllLandmarkCovariances();
+	void refineLinks();
+	void resetAllChanges();
+	void graphNodeSelected(int);
+	void graphLinkSelected(int, int);
+	void sliderAValueChanged(int);
+	void sliderBValueChanged(int);
+	void sliderAMoved(int);
+	void sliderBMoved(int);
+	void update3dView();
+	void sliderNeighborValueChanged(int);
+	void sliderLoopValueChanged(int);
+	void sliderIterationsValueChanged(int);
+	void editConstraint();
+	void updateGrid();
+	void updateOctomapView();
+	void updateGraphRotation();
+	void updateGraphView();
+	void refineConstraint();
+	void addConstraint();
+	void resetConstraint();
+	void rejectConstraint();
+	void updateConstraintView();
+	void updateLoggerLevel();
+	void updateStereo();
+	void notifyParametersChanged(const QStringList &);
+	void setupMainLayout(bool vertical);
+	void updateConstraintButtons();
+
+private:
+	QString getIniFilePath() const;
+	void readSettings();
+
+	void updateIds();
+	void update(int value,
+				QSpinBox * spinBoxIndex,
+				QLabel * labelParents,
+				QLabel * labelChildren,
+				QLabel * weight,
+				QLabel * label,
+				QLabel * stamp,
+				rtabmap::ImageView * view,
+				QLabel * labelId,
+				QLabel * labelMapId,
+				QLabel * labelPose,
+				QLabel * labelOptPose,
+				QLabel * labelVelocity,
+				QLabel * labelCalib,
+				QLabel * labelScan,
+				QLabel * labelGravity,
+				QLabel * labelPrior,
+				QToolButton * editPriorButton,
+				QToolButton * removePriorButton,
+				QLabel * labelGps,
+				QLabel * labelGt,
+				QLabel * labelSensors,
+				bool updateConstraintView);
+	void updateStereo(const SensorData * data);
+	void updateWordsMatching(const std::vector<int> & inliers = std::vector<int>());
+	void updateConstraintView(
+			const rtabmap::Link & link,
+			bool updateImageSliders = true,
+			const Signature & signatureFrom = Signature(0),
+			const Signature & signatureTo = Signature(0));
+	Link findActiveLink(int from, int to);
+	bool containsLink(
+			std::multimap<int, Link> & links,
+			int from,
+			int to);
+	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
+			const std::multimap<int, rtabmap::Link> & edgeConstraints);
+	void updateNeighborsSlider(int from = 0, int to = 0);
+	void updateLoopClosuresSlider(int from = 0, int to = 0);
+	void updateCovariances(const QList<Link> & links);
+	void refineLinks(const QList<Link> & links);
+	void refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regIcp, bool silent);
+	bool addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
+	void exportPoses(int format);
+	void exportGPS(int format);
+
+private:
+	Ui_DatabaseViewer * ui_;
+	CloudViewer * constraintsViewer_;
+	CloudViewer * cloudViewer_;
+	CloudViewer * stereoViewer_;
+	CloudViewer * occupancyGridViewer_;
+	QList<int> ids_;
+	std::set<int> lastWmIds_;
+	std::map<int, int> mapIds_;
+	std::map<int, int> weights_;
+	std::map<int, std::vector<int> > wmStates_;
+	QMap<int, int> idToIndex_;
+	QList<rtabmap::Link> neighborLinks_;
+	QList<rtabmap::Link> loopLinks_;
+	int lastSliderIndexBrowsed_;
+	rtabmap::DBDriver * dbDriver_;
+	QString pathDatabase_;
+	std::string databaseFileName_;
+	std::list<std::map<int, rtabmap::Transform> > graphes_;
+	std::multimap<int, rtabmap::Link> graphLinks_;
+	std::map<int, rtabmap::Transform> odomPoses_;
+	std::map<int, rtabmap::Transform> groundTruthPoses_;
+	std::map<int, rtabmap::Transform> gpsPoses_;
+	std::map<int, GPS> gpsValues_;
+	std::multimap<int, rtabmap::Link> links_;
+	std::multimap<int, rtabmap::Link> linksRefined_;
+	std::multimap<int, rtabmap::Link> linksAdded_;
+	std::multimap<int, rtabmap::Link> linksRemoved_;
+	std::map<int, LaserScan> modifiedLaserScans_;
+	std::vector<double> odomMaxInf_;
+	LocalGridCache localMaps_;
+	LocalGridCache generatedLocalMaps_;
+	OctoMap * octomap_;
+	ExportCloudsDialog * exportDialog_;
+	QDialog * editDepthDialog_;
+	EditDepthArea * editDepthArea_;
+	QDialog * editMapDialog_;
+	EditMapArea * editMapArea_;
+	LinkRefiningDialog * linkRefiningDialog_;
+
+	bool savedMaximized_;
+	bool firstCall_;
+	QString iniFilePath_;
+
+	bool infoReducedGraph_;
+	double infoTotalOdom_;
+	double infoTotalTime_;
+	int infoSessions_;
+};
+
+}
+
+#endif /* DATABASEVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/DepthCalibrationDialog.h

@@ -0,0 +1,86 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_DEPTHCALIBRATIONDIALOG_H_
+#define RTABMAP_DEPTHCALIBRATIONDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QMap>
+#include <QtCore/QSettings>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
+
+class Ui_DepthCalibrationDialog;
+
+namespace clams {
+class DiscreteDepthDistortionModel;
+}
+
+namespace rtabmap {
+
+class ProgressDialog;
+
+class RTABMAP_GUI_EXPORT DepthCalibrationDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	DepthCalibrationDialog(QWidget * parent = 0);
+	virtual ~DepthCalibrationDialog();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void calibrate(const std::map<int, Transform> & poses,
+			const QMap<int, Signature> & cachedSignatures,
+			const QString & workingDirectory,
+			const ParametersMap & parameters);
+
+Q_SIGNALS:
+	void configChanged();
+
+public Q_SLOTS:
+	void restoreDefaults();
+
+private Q_SLOTS:
+	void saveModel();
+	void cancel();
+
+private:
+	Ui_DepthCalibrationDialog * _ui;
+	ProgressDialog * _progressDialog;
+	bool _canceled;
+	clams::DiscreteDepthDistortionModel * _model;
+	QString _workingDirectory;
+	cv::Size _imageSize;
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_DEPTHCALIBRATIONDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/EditConstraintDialog.h

@@ -0,0 +1,63 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_EDITCONSTRAINTDIALOG_H_
+#define RTABMAP_EDITCONSTRAINTDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <rtabmap/core/Transform.h>
+
+class Ui_EditConstraintDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT EditConstraintDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1), QWidget * parent = 0);
+
+  void setPoseGroupVisible(bool visible);
+  void setCovarianceGroupVisible(bool visible);
+
+	virtual ~EditConstraintDialog();
+	Transform getTransform() const;
+	cv::Mat getCovariance() const;
+
+private Q_SLOTS:
+	void switchUnits();
+
+private:
+	Ui_EditConstraintDialog * _ui;
+};
+
+}
+
+#endif /* EDITCONSTRAINTDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/EditDepthArea.h

@@ -0,0 +1,100 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_EDITDEPTHAREA_H
+#define RTABMAP_EDITDEPTHAREA_H
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QColor>
+#include <QImage>
+#include <QPoint>
+#include <QWidget>
+#include <opencv2/opencv.hpp>
+#include "rtabmap/utilite/UCv2Qt.h"
+
+class QMenu;
+class QAction;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT EditDepthArea : public QWidget
+{
+    Q_OBJECT
+
+public:
+    EditDepthArea(QWidget *parent = 0);
+
+    void setImage(const cv::Mat & depth, const cv::Mat & rgb = cv::Mat());
+    cv::Mat getModifiedImage() const;
+    bool isModified() const {return modified_;}
+
+    void setPenWidth(int newWidth);
+    int penWidth() const { return myPenWidth_; }
+    void setClusterError(double error) { clusterError_ = error; }
+    double clusterError() const { return clusterError_; }
+    void setColorMap(uCvQtDepthColorMap type);
+
+public Q_SLOTS:
+    void resetChanges();
+
+protected:
+    virtual void mousePressEvent(QMouseEvent *event);
+    virtual void mouseMoveEvent(QMouseEvent *event);
+    virtual void mouseReleaseEvent(QMouseEvent *event);
+    virtual void paintEvent(QPaintEvent *event);
+    virtual void resizeEvent(QResizeEvent *event);
+    virtual void contextMenuEvent(QContextMenuEvent * e);
+
+private:
+    void drawLineTo(const QPoint &endPoint);
+    void computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const;
+
+    bool modified_;
+    bool scribbling_;
+    int myPenWidth_;
+    double clusterError_;
+    QImage imageRGB_;
+    QImage image_;
+    cv::Mat originalImage_;
+    QPoint lastPoint_;
+
+    QMenu * menu_;
+    QAction * showRGB_;
+    QAction * removeCluster_;
+    QAction * clusterErrorCluster_;
+    QAction * resetChanges_;
+    QAction * setPenWidth_;
+    QAction * colorMapWhiteToBlack_;
+	QAction * colorMapBlackToWhite_;
+	QAction * colorMapRedToBlue_;
+	QAction * colorMapBlueToRed_;
+};
+
+}
+
+#endif

rtabmap/guilib/include/rtabmap/gui/EditMapArea.h

@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_EDITMAPAREA_H
+#define RTABMAP_EDITMAPAREA_H
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QColor>
+#include <QImage>
+#include <QPoint>
+#include <QWidget>
+#include <opencv2/opencv.hpp>
+#include "rtabmap/utilite/UCv2Qt.h"
+
+class QMenu;
+class QAction;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT EditMapArea : public QWidget
+{
+    Q_OBJECT
+
+public:
+    EditMapArea(QWidget *parent = 0);
+
+    void setMap(const cv::Mat & map);
+    cv::Mat getModifiedMap() const;
+    bool isModified() const {return modified_;}
+
+    void setPenWidth(int newWidth);
+    int penWidth() const { return myPenWidth_; }
+    void setColorMap(uCvQtDepthColorMap type);
+
+public Q_SLOTS:
+    void resetChanges();
+
+protected:
+    virtual void mousePressEvent(QMouseEvent *event);
+    virtual void mouseMoveEvent(QMouseEvent *event);
+    virtual void mouseReleaseEvent(QMouseEvent *event);
+    virtual void paintEvent(QPaintEvent *event);
+    virtual void resizeEvent(QResizeEvent *event);
+    virtual void contextMenuEvent(QContextMenuEvent * e);
+
+private:
+    void drawLineTo(const QPoint &endPoint);
+    void computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const;
+
+    bool modified_;
+    bool scribbling_;
+    int myPenWidth_;
+    QImage map_;
+    cv::Mat originalMap_;
+    QPoint lastPoint_;
+
+    QMenu * menu_;
+    QAction * resetChanges_;
+    QAction * setPenWidth_;
+    QAction * addObstacle_;
+	QAction * clearObstacle_;
+	QAction * setUnknown_;
+};
+
+}
+
+#endif

rtabmap/guilib/include/rtabmap/gui/ExportBundlerDialog.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CORE_EXPORTBUNDLERDIALOG_H_
+#define RTABMAP_CORE_EXPORTBUNDLERDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
+#include <QDialog>
+#include <QSettings>
+
+class Ui_ExportBundlerDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT ExportBundlerDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	ExportBundlerDialog(QWidget * parent = 0);
+
+	virtual ~ExportBundlerDialog();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void setWorkingDirectory(const QString & path);
+
+	void exportBundler(
+			std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const QMap<int, Signature> & signatures,
+			const ParametersMap & parameters);
+
+Q_SIGNALS:
+	void configChanged();
+
+private Q_SLOTS:
+	void getPath();
+	void restoreDefaults();
+	void updateVisibility();
+
+private:
+	Ui_ExportBundlerDialog * _ui;
+};
+
+}
+
+#endif /* EXPORTBUNDLERDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -0,0 +1,164 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CORE_EXPORTCLOUDSDIALOG_H_
+#define RTABMAP_CORE_EXPORTCLOUDSDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QMap>
+#include <QtCore/QSettings>
+
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/TextureMesh.h>
+#include <pcl/pcl_base.h>
+
+class Ui_ExportCloudsDialog;
+class QAbstractButton;
+
+namespace rtabmap {
+class ProgressDialog;
+class GainCompensator;
+class DBDriver;
+
+class RTABMAP_GUI_EXPORT ExportCloudsDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	ExportCloudsDialog(QWidget *parent = 0);
+
+	virtual ~ExportCloudsDialog();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
+	void forceAssembling(bool enabled);
+	void setProgressDialogToMax();
+	void setSaveButton();
+	void setOkButton();
+
+	void exportClouds(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, int> & mapIds,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const QString & workingDirectory,
+			const ParametersMap & parameters);
+
+	void viewClouds(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, int> & mapIds,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const QString & workingDirectory,
+			const ParametersMap & parameters);
+
+	bool getExportedClouds(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, int> & mapIds,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const QString & workingDirectory,
+			const ParametersMap & parameters,
+			std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+			std::map<int, pcl::PolygonMesh::Ptr> & meshes,
+			std::map<int, pcl::TextureMesh::Ptr> & textureMeshes,
+			std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
+
+	int getTextureSize() const;
+	int getMaxTextures() const;
+	bool isGainCompensation() const;
+	double getGainBeta() const;
+	bool isGainRGB() const;
+	bool isBlending() const;
+	int getBlendingDecimation() const;
+	int getTextureBrightnessConstrastRatioLow() const;
+	int getTextureBrightnessConstrastRatioHigh() const;
+	bool isExposeFusion() const;
+
+	static bool removeDirRecursively(const QString & dirName);
+
+Q_SIGNALS:
+	void configChanged();
+
+public Q_SLOTS:
+	void restoreDefaults();
+
+private Q_SLOTS:
+	void loadSettings();
+	void saveSettings();
+	void updateReconstructionFlavor();
+	void selectDistortionModel();
+	void selectCamProjMask();
+	void updateMLSGrpVisibility();
+	void cancel();
+
+private:
+	std::map<int, Transform> filterNodes(const std::map<int, Transform> & poses);
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
+			const std::map<int, Transform> & poses,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const ParametersMap & parameters,
+			bool & has2dScans,
+			bool & scansHaveRGB) const;
+	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true, const std::vector<std::map<int, pcl::PointXY> > & pointToPixels = std::vector<std::map<int, pcl::PointXY> >());
+	void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
+	void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
+
+private:
+	Ui_ExportCloudsDialog * _ui;
+	ProgressDialog * _progressDialog;
+	QString _workingDirectory;
+	bool _canceled;
+	GainCompensator * _compensator;
+	const DBDriver * _dbDriver;
+	bool _scansHaveRGB;
+
+    bool saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const;
+    bool saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const;
+
+};
+
+}
+
+#endif /* EXPORTCLOUDSDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/ExportDialog.h

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CORE_EXPORTDIALOG_H_
+#define RTABMAP_CORE_EXPORTDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QSettings>
+
+class Ui_ExportDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT ExportDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	ExportDialog(QWidget * parent = 0);
+
+	virtual ~ExportDialog();
+
+	void saveSettings(QSettings & settings, const QString & group) const;
+	void loadSettings(QSettings & settings, const QString & group);
+
+	QString outputPath() const;
+	int framesIgnored() const;
+	double targetFramerate() const;
+	int sessionExported() const;
+	bool isRgbExported() const;
+	bool isDepthExported() const;
+	bool isDepth2dExported() const;
+	bool isOdomExported() const;
+	bool isUserDataExported() const;
+
+Q_SIGNALS:
+	void configChanged();
+
+private Q_SLOTS:
+	void getPath();
+	void restoreDefaults();
+
+private:
+	Ui_ExportDialog * _ui;
+};
+
+}
+
+#endif /* EXPORTDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/GraphViewer.h

@@ -0,0 +1,249 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_GRAPHVIEWER_H_
+#define RTABMAP_GRAPHVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QGraphicsView>
+#include <QtCore/QMap>
+#include <QtCore/QSettings>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/GPS.h>
+#include <opencv2/opencv.hpp>
+#include <map>
+#include <set>
+#include <vector>
+
+class QGraphicsItem;
+class QGraphicsPixmapItem;
+class QGraphicsItemGroup;
+
+namespace rtabmap {
+
+class NodeItem;
+class LinkItem;
+
+class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView {
+
+	Q_OBJECT;
+
+public:
+	enum ViewPlane {XY, XZ, YZ};
+
+public:
+	GraphViewer(QWidget * parent = 0);
+	virtual ~GraphViewer();
+
+	void setWorldMapRotation(const float & theta);
+	float getWorldMapRotation() const {return _worldMapRotation;}
+
+	void updateGraph(const std::map<int, Transform> & poses,
+					 const std::multimap<int, Link> & constraints,
+					 const std::map<int, int> & mapIds,
+					 const std::map<int, int> & weights = std::map<int, int>(),
+					 const std::set<int> & odomCacheIds = std::set<int>());
+	void updateGTGraph(const std::map<int, Transform> & poses);
+	void updateGPSGraph(
+			const std::map<int, Transform> & gpsMapPoses,
+			const std::map<int, GPS> & gpsValues);
+	void updateReferentialPosition(const Transform & t);
+	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
+	// Use updateNodeColorByValue() instead with valueName="Posterior".
+	RTABMAP_DEPRECATED void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
+	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMax = 0.0f, bool invertedColorScale = false, int zValueOffset = 0);
+	void updateLocalPath(const std::vector<int> & localPath);
+	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
+	void setCurrentGoalID(int id, const Transform & pose = Transform());
+	void setNodeInfo(int id, const QString & info);
+	void setLocalRadius(float radius);
+	void highlightNode(int nodeId, int highlightIndex);
+	void clearGraph();
+	void clearMap();
+	// Use clearNodeColorByValue() instead.
+	RTABMAP_DEPRECATED void clearPosterior();
+	void clearNodeColorByValue();
+	void clearAll();
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	//getters
+	const QString & getWorkingDirectory() const {return _workingDirectory;}
+	float getNodeRadius() const {return _nodeRadius;}
+	float getLinkWidth() const {return _linkWidth;}
+	const QColor & getNodeColor() const {return _nodeColor;}
+	const QColor & getNodeOdomCacheColor() const {return _nodeOdomCacheColor;}
+	const QColor & getCurrentGoalColor() const {return _currentGoalColor;}
+	const QColor & getNeighborColor() const {return _neighborColor;}
+	const QColor & getGlobalLoopClosureColor() const {return _loopClosureColor;}
+	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
+	const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
+	const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
+	const QColor & getNeighborMergedColor() const {return _neighborMergedColor;}
+	const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;}
+	const QColor & getLocalPathColor() const {return _localPathColor;}
+	const QColor & getGlobalPathColor() const {return _globalPathColor;}
+	const QColor & getGTColor() const {return _gtPathColor;}
+	const QColor & getGPSColor() const {return _gpsPathColor;}
+	const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;}
+	const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;}
+	bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;}
+	bool isGridMapVisible() const;
+	bool isOriginVisible() const;
+	bool isReferentialVisible() const;
+	bool isLocalRadiusVisible() const;
+	float getLoopClosureOutlierThr() const {return _loopClosureOutlierThr;}
+	float getMaxLinkLength() const {return _maxLinkLength;}
+	bool isGraphVisible() const;
+	bool isGlobalPathVisible() const;
+	bool isLocalPathVisible() const;
+	bool isGtGraphVisible() const;
+	bool isGPSGraphVisible() const;
+	bool isOdomCacheOverlayVisible() const;
+	bool isOrientationENU() const;
+	ViewPlane getViewPlane() const;
+	bool isEnsureFrameVisible() const;
+
+	// setters
+	void setWorkingDirectory(const QString & path);
+	void setNodeVisible(bool visible);
+	void setNodeRadius(float radius);
+	void setLinkWidth(float width);
+	void setNodeColor(const QColor & color);
+	void setNodeOdomCacheColor(const QColor & color);
+	void setCurrentGoalColor(const QColor & color);
+	void setNeighborColor(const QColor & color);
+	void setGlobalLoopClosureColor(const QColor & color);
+	void setLocalLoopClosureColor(const QColor & color);
+	void setUserLoopClosureColor(const QColor & color);
+	void setVirtualLoopClosureColor(const QColor & color);
+	void setNeighborMergedColor(const QColor & color);
+	void setLandmarkColor(const QColor & color);
+	void setRejectedLoopClosureColor(const QColor & color);
+	void setLocalPathColor(const QColor & color);
+	void setGlobalPathColor(const QColor & color);
+	void setGTColor(const QColor & color);
+	void setGPSColor(const QColor & color);
+	void setHighlightColor(const QColor & color, int index);
+	void setIntraSessionLoopColor(const QColor & color);
+	void setInterSessionLoopColor(const QColor & color);
+	void setIntraInterSessionColorsEnabled(bool enabled);
+	void setGridMapVisible(bool visible);
+	void setOriginVisible(bool visible);
+	void setReferentialVisible(bool visible);
+	void setLocalRadiusVisible(bool visible);
+	void setLoopClosureOutlierThr(float value);
+	void setMaxLinkLength(float value);
+	void setGraphVisible(bool visible);
+	void setGlobalPathVisible(bool visible);
+	void setLocalPathVisible(bool visible);
+	void setGtGraphVisible(bool visible);
+	void setGPSGraphVisible(bool visible);
+	void setOdomCacheOverlayVisible(bool visible);
+	void setOrientationENU(bool enabled);
+	void setViewPlane(ViewPlane plane);
+	void setEnsureFrameVisible(bool visible);
+
+Q_SIGNALS:
+	void configChanged();
+	void mapShownRequested();
+	void nodeSelected(int);
+	void linkSelected(int, int);
+
+public Q_SLOTS:
+	void restoreDefaults();
+
+protected:
+	virtual void wheelEvent ( QWheelEvent * event );
+	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void mousePressEvent(QMouseEvent * event);
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+
+private:
+	QString _workingDirectory;
+	QColor _nodeColor;
+	QColor _nodeOdomCacheColor;
+	QColor _currentGoalColor;
+	QColor _neighborColor;
+	QColor _loopClosureColor;
+	QColor _loopClosureLocalColor;
+	QColor _loopClosureUserColor;
+	QColor _loopClosureVirtualColor;
+	QColor _neighborMergedColor;
+	QColor _landmarkColor;
+	QColor _loopClosureRejectedColor;
+	QColor _localPathColor;
+	QColor _globalPathColor;
+	QColor _gtPathColor;
+	QColor _gpsPathColor;
+	QColor _loopIntraSessionColor;
+	QColor _loopInterSessionColor;
+	bool _intraInterSessionColors;
+	float _worldMapRotation;
+	QGraphicsItem * _world;
+	QGraphicsItem * _root;
+	QGraphicsItem * _graphRoot;
+	QGraphicsItem * _globalPathRoot;
+	QGraphicsItem * _localPathRoot;
+	QGraphicsItem * _gtGraphRoot;
+	QGraphicsItem * _gpsGraphRoot;
+	QMap<int, NodeItem*> _nodeItems;
+	QMultiMap<int, LinkItem*> _linkItems;
+	QMap<int, NodeItem*> _gtNodeItems;
+	QMap<int, NodeItem*> _gpsNodeItems;
+	QMultiMap<int, LinkItem*> _gtLinkItems;
+	QMultiMap<int, LinkItem*> _gpsLinkItems;
+	QMultiMap<int, LinkItem*> _localPathLinkItems;
+	QMultiMap<int, LinkItem*> _globalPathLinkItems;
+	QVector<QPair<QColor, NodeItem*> > _highlightedNodes;
+	bool _nodeVisible;
+	float _nodeRadius;
+	float _linkWidth;
+	QGraphicsPixmapItem * _gridMap;
+	QGraphicsItemGroup * _referential;
+	QGraphicsItemGroup * _referentialXY;
+	QGraphicsItemGroup * _referentialXZ;
+	QGraphicsItemGroup * _referentialYZ;
+	QGraphicsItemGroup * _originReferential;
+	QGraphicsItemGroup * _originReferentialXY;
+	QGraphicsItemGroup * _originReferentialXZ;
+	QGraphicsItemGroup * _originReferentialYZ;
+	float _gridCellSize;
+	QGraphicsEllipseItem * _localRadius;
+	QGraphicsRectItem * _odomCacheOverlay;
+	float _loopClosureOutlierThr;
+	float _maxLinkLength;
+	bool _orientationENU;
+	bool _mouseTracking;
+	ViewPlane _viewPlane;
+	bool _ensureFrameVisible;
+	QPoint _previousMousePos;
+};
+
+} /* namespace rtabmap */
+#endif /* GRAPHVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/ImageView.h

@@ -0,0 +1,193 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_IMAGEVIEW_H_
+#define RTABMAP_IMAGEVIEW_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QGraphicsView>
+#include <QtCore/QRectF>
+#include <QtCore/QMultiMap>
+#include <QtCore/QSettings>
+#include <opencv2/features2d/features2d.hpp>
+#include <map>
+#include "rtabmap/utilite/UCv2Qt.h"
+#include <rtabmap/core/CameraModel.h>
+
+class QAction;
+class QMenu;
+
+namespace rtabmap {
+
+class KeypointItem;
+
+class RTABMAP_GUI_EXPORT ImageView : public QWidget {
+
+	Q_OBJECT
+
+public:
+	ImageView(QWidget * parent = 0);
+	virtual ~ImageView();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	QRectF sceneRect() const;
+	bool isImageShown() const;
+	bool isImageDepthShown() const;
+	bool isImageDepthConfidenceShown() const;
+	bool isFeaturesShown() const;
+	bool isLinesShown() const;
+	int getAlpha() const {return _alpha;}
+	int getFeaturesSize() const {return _featuresSize;}
+	int getLinesWidth() const {return _linesWidth;}
+	bool isGraphicsViewMode() const;
+	bool isGraphicsViewScaled() const;
+	bool isGraphicsViewScaledToHeight() const;
+	const QColor & getDefaultBackgroundColor() const;
+	const QColor & getDefaultFeatureColor() const;
+	const QColor & getDefaultMatchingFeatureColor() const;
+	const QColor & getDefaultMatchingLineColor() const;
+	const QColor & getBackgroundColor() const;
+	float getDepthColorMapMinRange() const;
+	float getDepthColorMapMaxRange() const;
+	uCvQtDepthColorMap getDepthColorMap() const;
+
+	float viewScale() const;
+
+	void setFeaturesShown(bool shown);
+	void setImageShown(bool shown);
+	void setImageDepthShown(bool shown);
+	void setImageDepthConfidenceShown(bool shown);
+	void setLinesShown(bool shown);
+	void setGraphicsViewMode(bool on);
+	void setGraphicsViewScaled(bool scaled);
+	void setGraphicsViewScaledToHeight(bool scaled);
+	void setDefaultBackgroundColor(const QColor & color);
+	void setDefaultFeatureColor(const QColor & color);
+	void setDefaultMatchingFeatureColor(const QColor & color);
+	void setDefaultMatchingLineColor(const QColor & color);
+	void setBackgroundColor(const QColor & color);
+	void setDepthColorMapRange(float min, float max);
+
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
+	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
+	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
+	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
+	void setImage(const QImage & image, const std::vector<CameraModel> & models = std::vector<CameraModel>(), const Transform & pose = Transform());
+	void setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageDepthConfidence = cv::Mat());
+	void setImageDepth(const QImage & image, const QImage & imageDepthConfidence = QImage());
+	void setFeatureColor(int id, QColor color);
+	void setFeaturesColor(QColor color);
+	void setAlpha(int alpha);
+	void setFeaturesSize(int size);
+	void setLinesWidth(int width);
+	void setSceneRect(const QRectF & rect);
+
+	const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
+
+	void clearLines();
+	void clearFeatures();
+	void clear();
+
+	virtual QSize sizeHint() const;
+
+Q_SIGNALS:
+	void configChanged();
+
+protected:
+	virtual void paintEvent(QPaintEvent *event);
+	virtual void resizeEvent(QResizeEvent* event);
+	virtual void contextMenuEvent(QContextMenuEvent * e);
+	virtual void mouseMoveEvent(QMouseEvent * event);
+
+private Q_SLOTS:
+	void sceneRectChanged(const QRectF &rect);
+
+private:
+	void updateOpacity();
+	void computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const;
+	QIcon createIcon(const QColor & color);
+
+private:
+	QString _savedFileName;
+	int _alpha;
+	int _featuresSize;
+	int _linesWidth;
+	QColor _defaultBgColor;
+	QColor _defaultFeatureColor;
+	QColor _defaultMatchingFeatureColor;
+	QColor _defaultMatchingLineColor;
+	float _depthColorMapMinRange;
+	float _depthColorMapMaxRange;
+
+	QMenu * _menu;
+	QAction * _showImage;
+	QAction * _showImageDepth;
+	QAction * _showImageDepthConfidence;
+	QAction * _showFeatures;
+	QAction * _showLines;
+	QAction * _setFeatureColor;
+	QAction * _setMatchingFeatureColor;
+	QAction * _setMatchingLineColor;
+	QAction * _saveImage;
+	QAction * _setAlpha;
+	QAction * _setFeaturesSize;
+	QAction * _setLinesWidth;
+	QAction * _graphicsViewMode;
+	QAction * _graphicsViewScaled;
+	QAction * _graphicsViewScaledToHeight;
+	QAction * _graphicsViewNoScaling;
+	QAction * _colorMapWhiteToBlack;
+	QAction * _colorMapBlackToWhite;
+	QAction * _colorMapRedToBlue;
+	QAction * _colorMapBlueToRed;
+	QAction * _colorMapMinRange;
+	QAction * _colorMapMaxRange;
+	QAction * _mouseTracking;
+	QMenu * _featureMenu;
+	QMenu * _scaleMenu;
+
+	QGraphicsView * _graphicsView;
+	QMultiMap<int, rtabmap::KeypointItem *> _features;
+	QList<QGraphicsLineItem*> _lines;
+	QGraphicsPixmapItem * _imageItem;
+	QGraphicsPixmapItem * _imageDepthItem;
+	QGraphicsPixmapItem * _imageDepthConfidenceItem;
+	QPixmap _image;
+	QPixmap _imageDepth;
+	QPixmap _imageDepthConfidence;
+	cv::Mat _imageDepthCv;
+	cv::Mat _imageDepthConfidenceCv;
+	std::vector<CameraModel> _models;
+	Transform _pose;
+};
+
+}
+
+#endif /* IMAGEVIEW_H_ */

rtabmap/guilib/include/rtabmap/gui/KeypointItem.h

@@ -0,0 +1,70 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_KEYPOINTITEM_H_
+#define RTABMAP_KEYPOINTITEM_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QGraphicsEllipseItem>
+#include <QGraphicsTextItem>
+#include <QtGui/QPen>
+#include <QtGui/QBrush>
+#include <opencv2/features2d/features2d.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT KeypointItem : public QGraphicsEllipseItem
+{
+public:
+	KeypointItem(int id, const cv::KeyPoint & kpt, float depth = 0, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
+	virtual ~KeypointItem();
+
+	void setColor(const QColor & color);
+	const cv::KeyPoint & keypoint() const {return _kpt;}
+
+protected:
+	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event );
+	virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event );
+	virtual void focusInEvent ( QFocusEvent * event );
+	virtual void focusOutEvent ( QFocusEvent * event );
+
+private:
+	void showDescription();
+	void hideDescription();
+
+private:
+	int _id;
+	cv::KeyPoint _kpt;
+	QGraphicsRectItem * _placeHolder;
+	int _width;
+	float _depth;
+};
+
+}
+
+#endif /* KEYPOINTITEM_H_ */

rtabmap/guilib/include/rtabmap/gui/LinkRefiningDialog.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_LINKREFININGDIALOG_H_
+#define RTABMAP_LINKREFININGDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Link.h>
+
+#include <QDialog>
+
+class Ui_linkRefiningDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT LinkRefiningDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	LinkRefiningDialog(QWidget * parent = 0);
+
+	virtual ~LinkRefiningDialog();
+
+	void setMinMax(
+		int nodeIdMin,
+		int nodeIdMax,
+		int mapIdMin,
+		int mapIdMax);
+
+	Link::Type getLinkType() const;
+	void getIntraInterSessions(bool & intra, bool & inter) const;
+	bool isRangeByNodeId() const;
+	bool isRangeByMapId() const;
+	void getRangeNodeId(int & from, int & to) const;
+	void getRangeMapId(int & from, int & to) const;
+
+private Q_SLOTS:
+	void restoreDefaults();
+	void updateIntraInterState();
+	void setRangeToNodeId();
+	void setRangeToMapId();
+
+private:
+	Ui_linkRefiningDialog * ui_;
+	int defaultNodeIdMin_;
+	int defaultNodeIdMax_;
+	int defaultMapIdMin_;
+	int defaultMapIdMax_;
+};
+
+}
+
+#endif /* RTABMAP_LINKREFININGDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_LOOPCLOSUREVIEWER_H_
+#define RTABMAP_LOOPCLOSUREVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Signature.h>
+#include <opencv2/opencv.hpp>
+#include <QWidget>
+
+class Ui_loopClosureViewer;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT LoopClosureViewer : public QWidget {
+
+	Q_OBJECT
+
+public:
+	LoopClosureViewer(QWidget * parent = 0);
+	virtual ~LoopClosureViewer();
+
+	void setData(const Signature & sA, const Signature & sB); // sB contains loop transform as pose() from sA
+
+	const Signature & sA() const {return sA_;}
+	const Signature & sB() const {return sB_;}
+
+public Q_SLOTS:
+	void setDecimation(int decimation) {decimation_ = decimation;}
+	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
+	void setMinDepth(int minDepth) {minDepth_ = minDepth;}
+	void updateView(const Transform & AtoB = Transform(), const ParametersMap & parameters = ParametersMap());
+
+protected:
+	virtual void showEvent(QShowEvent * event);
+
+private:
+	Ui_loopClosureViewer * ui_;
+
+	Signature sA_;
+	Signature sB_;
+	Transform transform_;
+
+	int decimation_;
+	float maxDepth_;
+	float minDepth_;
+};
+
+} /* namespace rtabmap */
+#endif /* LOOPCLOSUREVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/MainWindow.h

@@ -0,0 +1,436 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_MAINWINDOW_H_
+#define RTABMAP_MAINWINDOW_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UEventsHandler.h"
+#include <QMainWindow>
+#include <QtCore/QSet>
+#include <QElapsedTimer>
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Optimizer.h"
+#include "rtabmap/core/GlobalMap.h"
+#include "rtabmap/gui/PreferencesDialog.h"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
+#include <rtabmap/core/SensorCaptureInfo.h>
+
+namespace rtabmap {
+class SensorCaptureThread;
+class OdometryThread;
+class IMUThread;
+class CloudViewer;
+class LoopClosureViewer;
+class OccupancyGrid;
+}
+
+class QGraphicsScene;
+class Ui_mainWindow;
+class QActionGroup;
+
+namespace rtabmap {
+
+class LikelihoodScene;
+class AboutDialog;
+class Plot;
+class PdfPlotCurve;
+class StatsToolBox;
+class ProgressDialog;
+class TwistGridWidget;
+class ExportCloudsDialog;
+class ExportBundlerDialog;
+class PostProcessingDialog;
+class DepthCalibrationDialog;
+class DataRecorder;
+class OctoMap;
+class GridMap;
+class MultiSessionLocWidget;
+
+class RTABMAP_GUI_EXPORT MainWindow : public QMainWindow, public UEventsHandler
+{
+	Q_OBJECT
+
+public:
+	enum State {
+		kIdle,
+		kInitializing,
+		kInitialized,
+		kApplicationClosing,
+		kClosing,
+		kStartingDetection,
+		kDetecting,
+		kPaused,
+		kMonitoring,
+		kMonitoringPaused
+	};
+
+public:
+	/**
+	 * @param prefDialog If NULL, a default dialog is created. This
+	 *                   dialog is automatically destroyed with the MainWindow.
+	 */
+	MainWindow(PreferencesDialog * prefDialog = 0, QWidget * parent = 0, bool showSplashScreen = true);
+	virtual ~MainWindow();
+
+	QString getWorkingDirectory() const;
+	void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
+	bool isSavedMaximized() const {return _savedMaximized;}
+
+	bool isProcessingStatistics() const {return _processingStatistics;}
+	bool isProcessingOdometry() const {return _processingOdometry;}
+
+	bool isDatabaseUpdated() const { return _databaseUpdated; }
+
+public Q_SLOTS:
+	virtual void processStats(const rtabmap::Statistics & stat);
+	void updateCacheFromDatabase(const QString & path);
+	void openDatabase(const QString & path, const rtabmap::ParametersMap & overridedParameters = rtabmap::ParametersMap());
+	void updateParameters(const rtabmap::ParametersMap & parameters);
+
+protected:
+	virtual void closeEvent(QCloseEvent* event);
+	virtual bool handleEvent(UEvent* anEvent);
+	virtual void showEvent(QShowEvent* anEvent);
+	virtual void moveEvent(QMoveEvent* anEvent);
+	virtual void resizeEvent(QResizeEvent* anEvent);
+	virtual void keyPressEvent(QKeyEvent *event);
+	virtual bool eventFilter(QObject *obj, QEvent *event);
+
+protected Q_SLOTS:
+	virtual void changeState(MainWindow::State state);
+	virtual void newDatabase();
+	virtual void openDatabase();
+	virtual bool closeDatabase();
+	virtual void startDetection();
+	virtual void pauseDetection();
+	virtual void stopDetection();
+	virtual void saveConfigGUI();
+	virtual void downloadAllClouds();
+	virtual void downloadPoseGraph();
+	virtual void clearTheCache();
+	virtual void openHelp();
+	virtual void openPreferences();
+	virtual void openPreferencesSource();
+	virtual void setDefaultViews();
+	virtual void resetOdometry();
+	virtual void triggerNewMap();
+	virtual void deleteMemory();
+
+protected Q_SLOTS:
+	void beep();
+	void cancelProgress();
+	void configGUIModified();
+	void editDatabase();
+	void notifyNoMoreImages();
+	void printLoopClosureIds();
+	void generateGraphDOT();
+	void exportPosesRaw();
+	void exportPosesRGBDSLAM();
+	void exportPosesRGBDSLAMMotionCapture();
+	void exportPosesRGBDSLAMID();
+	void exportPosesKITTI();
+	void exportPosesTORO();
+	void exportPosesG2O();
+	void exportImages();
+	void exportOctomap();
+	void showPostProcessingDialog();
+	void depthCalibration();
+	void openWorkingDirectory();
+	void updateEditMenu();
+	void selectOpenni();
+	void selectFreenect();
+	void selectOpenniCv();
+	void selectOpenniCvAsus();
+	void selectOpenni2();
+	void selectFreenect2();
+	void selectK4W2();
+	void selectK4A();
+	void selectRealSense();
+	void selectRealSense2();
+	void selectRealSense2L515();
+	void selectRealSense2Stereo();
+	void selectStereoDC1394();
+	void selectStereoFlyCapture2();
+	void selectStereoZed();
+	void selectStereoZedOC();
+	void selectStereoTara();
+	void selectStereoUsb();
+	void selectMyntEyeS();
+	void selectDepthAIOAKD();
+	void selectDepthAIOAKDLite();
+	void selectDepthAIOAKDPro();
+	void selectXvisioSeerSense();
+	void selectVLP16();
+	void dumpTheMemory();
+	void dumpThePrediction();
+	void sendGoal();
+	void sendWaypoints();
+	void postGoal();
+	void postGoal(const QString & goal);
+	void cancelGoal();
+	void label();
+	void removeLabel();
+	void updateCacheFromDatabase();
+	void anchorCloudsToGroundTruth();
+	void selectScreenCaptureFormat(bool checked);
+	void takeScreenshot();
+	void updateElapsedTime();
+	void processCameraInfo(const rtabmap::SensorCaptureInfo & info);
+	void processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored);
+	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 processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
+	void processRtabmapLabelErrorEvent(int id, const QString & label);
+	void processRtabmapGoalStatusEvent(int status);
+	void changeImgRateSetting();
+	void changeDetectionRateSetting();
+	void changeTimeLimitSetting();
+	void changeMappingMode();
+	void setAspectRatio(int w, int h);
+	void setAspectRatio16_9();
+	void setAspectRatio16_10();
+	void setAspectRatio4_3();
+	void setAspectRatio240p();
+	void setAspectRatio360p();
+	void setAspectRatio480p();
+	void setAspectRatio720p();
+	void setAspectRatio1080p();
+	void setAspectRatioCustom();
+	void exportGridMap();
+	void exportClouds();
+	void exportBundlerFormat();
+	void viewClouds();
+	void dataRecorder();
+	void dataRecorderDestroyed();
+	void updateNodeVisibility(int, bool);
+	void updateGraphView();
+
+Q_SIGNALS:
+	void statsReceived(const rtabmap::Statistics &);
+	void statsProcessed();
+	void cameraInfoReceived(const rtabmap::SensorCaptureInfo &);
+	void cameraInfoProcessed();
+	void odometryReceived(const rtabmap::OdometryEvent &, bool);
+	void odometryProcessed();
+	void thresholdsChanged(int, int);
+	void stateChanged(MainWindow::State);
+	void rtabmapEventInitReceived(int status, const QString & info);
+	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
+	void rtabmapEvent3DMapProcessed();
+	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
+	void rtabmapLabelErrorReceived(int id, const QString & label);
+	void rtabmapGoalStatusEventReceived(int status);
+	void imgRateChanged(double);
+	void detectionRateChanged(double);
+	void timeLimitChanged(float);
+	void mappingModeChanged(bool);
+	void noMoreImagesReceived();
+	void loopClosureThrChanged(qreal);
+	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 std::multimap<int, Link> & constraints,
+			const std::map<int, int> & mapIds,
+			const std::map<int, std::string> & labels,
+			const std::map<int, Transform> & groundTruths,
+			const std::map<int, Transform> & odomCachePoses = std::map<int, Transform>(),
+			const std::multimap<int, Link> & odomCacheConstraints = std::multimap<int, Link>(),
+			bool verboseProgress = false,
+			std::map<std::string, float> * stats = 0);
+	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
+	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
+	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
+	Transform alignPosesToGroundTruth(const std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
+	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
+	void drawLandmarks(cv::Mat & image, const Signature & signature);
+	void setupMainLayout(bool vertical);
+	void updateSelectSourceMenu();
+	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
+	void saveFigures();
+	void loadFigures();
+	void exportPoses(int format);
+	QString captureScreen(bool cacheInRAM = false, bool png = true);
+
+protected:
+	Ui_mainWindow * ui() { return _ui; }
+	const State & state() const { return _state; }
+
+	const QMap<int, Signature> & cachedSignatures() const { return _cachedSignatures;}
+	const std::map<int, Transform> & currentPosesMap() const { return _currentPosesMap; }  // <nodeId, pose>
+	const std::map<int, Transform> & currentGTPosesMap() const { return _currentGTPosesMap; }  // <nodeId, pose>
+	std::map<int, Transform> currentVisiblePosesMap() const; // <nodeId, pose>
+	const std::multimap<int, Link> & currentLinksMap() const { return _currentLinksMap; }  // <nodeFromId, link>
+	const std::map<int, int> & currentMapIds() const { return _currentMapIds; }    // <nodeId, mapId>
+	const std::map<int, std::string> & currentLabels() const { return _currentLabels; }  // <nodeId, label>
+	const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds() const { return _cachedClouds; }
+	const std::map<int, LaserScan> & createdScans() const { return _createdScans; }
+	const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & createdFeatures() const { return _createdFeatures; }
+
+	const rtabmap::OccupancyGrid * occupancyGrid() const { return _occupancyGrid; }
+	const rtabmap::OctoMap * octomap() const { return _octomap; }
+
+	rtabmap::ProgressDialog * progressDialog() { return _progressDialog; }
+	rtabmap::CloudViewer * cloudViewer() const { return _cloudViewer; }
+	rtabmap::LoopClosureViewer * loopClosureViewer() const { return _loopClosureViewer; }
+
+	void setCloudViewer(rtabmap::CloudViewer * cloudViewer);
+	void setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer);
+
+	void setNewDatabasePathOutput(const QString & newDatabasePathOutput) {_newDatabasePathOutput = newDatabasePathOutput;}
+	const QString & newDatabasePathOutput() const { return _newDatabasePathOutput; }
+
+	virtual ParametersMap getCustomParameters() {return ParametersMap();}
+
+	void postProcessing(
+			bool refineNeighborLinks,
+			bool refineLoopClosureLinks,
+			// Detect more loop closures params:
+			bool detectMoreLoopClosures,
+			double clusterRadius,
+			double clusterAngle,
+			int iterations,
+			bool interSession,
+			bool intraSession,
+			// SBA params:
+			bool sba,
+			int sbaIterations,
+			double sbaVariance,
+			Optimizer::Type sbaType,
+			double sbaRematchFeatures,
+			bool abortIfDataMissing = true);
+
+private:
+	Ui_mainWindow * _ui;
+
+	State _state;
+	rtabmap::SensorCaptureThread * _sensorCapture;
+	rtabmap::OdometryThread * _odomThread;
+	rtabmap::IMUThread * _imuThread;
+
+	//Dialogs
+	PreferencesDialog * _preferencesDialog;
+	AboutDialog * _aboutDialog;
+	ExportCloudsDialog * _exportCloudsDialog;
+	ExportBundlerDialog * _exportBundlerDialog;
+	PostProcessingDialog * _postProcessingDialog;
+	DepthCalibrationDialog * _depthCalibrationDialog;
+	DataRecorder * _dataRecorder;
+
+	QSet<int> _lastIds;
+	int _lastId;
+	double _firstStamp;
+	bool _processingStatistics;
+	bool _processingDownloadedMap;
+	bool _recovering;
+	bool _odometryReceived;
+	QString _newDatabasePath;
+	QString _newDatabasePathOutput;
+	QString _openedDatabasePath;
+	QString _defaultOpenDatabasePath;
+	bool _databaseUpdated;
+	bool _odomImageShow;
+	bool _odomImageDepthShow;
+	bool _savedMaximized;
+	QStringList _waypoints;
+	int _waypointsIndex;
+	std::vector<CameraModel> _rectCameraModels;
+	std::vector<CameraModel> _rectCameraModelsOdom;
+
+	QMap<int, Signature> _cachedSignatures;
+	long _cachedMemoryUsage;
+	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
+	std::map<int, Transform> _currentGTPosesMap; // <nodeId, pose>
+	std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
+	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
+	std::map<int, std::string> _currentLabels; // <nodeId, label>
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > _cachedClouds;
+	long _createdCloudsMemoryUsage;
+	std::set<int> _cachedEmptyClouds;
+	std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
+	std::map<int, float> _cachedWordsCount;
+	std::map<int, float> _cachedLocalizationsCount;
+	rtabmap::LocalGridCache _cachedLocalMaps;
+
+	std::map<int, LaserScan> _createdScans;
+
+	rtabmap::OccupancyGrid * _occupancyGrid;
+	rtabmap::OctoMap * _octomap;
+	rtabmap::GridMap * _elevationMap;
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;
+
+	Transform _odometryCorrection;
+	Transform _lastOdomPose;
+	bool _processingOdometry;
+
+	QTimer * _oneSecondTimer;
+	QElapsedTimer * _elapsedTime;
+	QElapsedTimer * _logEventTime;
+
+	PdfPlotCurve * _posteriorCurve;
+	PdfPlotCurve * _likelihoodCurve;
+	PdfPlotCurve * _rawLikelihoodCurve;
+
+	MultiSessionLocWidget * _multiSessionLocWidget;
+
+	ProgressDialog * _progressDialog;
+
+	CloudViewer * _cloudViewer;
+	LoopClosureViewer * _loopClosureViewer;
+
+	QString _graphSavingFileName;
+	int _exportPosesFrame;
+	QMap<int, QString> _exportPosesFileName;
+	bool _autoScreenCaptureOdomSync;
+	bool _autoScreenCaptureRAM;
+	bool _autoScreenCapturePNG;
+	QMap<QString, QByteArray> _autoScreenCaptureCachedImages;
+
+	QVector<int> _refIds;
+	QVector<int> _loopClosureIds;
+
+	bool _firstCall;
+	bool _progressCanceled;
+};
+
+}
+
+#endif /* RTABMAP_MainWindow_H_ */

rtabmap/guilib/include/rtabmap/gui/MapVisibilityWidget.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_MAPVISIBILITYWIDGET_H_
+#define RTABMAP_MAPVISIBILITYWIDGET_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QWidget>
+#include <rtabmap/core/Transform.h>
+#include <map>
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT MapVisibilityWidget : public QWidget {
+	Q_OBJECT;
+public:
+	MapVisibilityWidget(QWidget * parent = 0);
+	virtual ~MapVisibilityWidget();
+
+	void setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask);
+	std::map<int, Transform> getVisiblePoses() const;
+
+	void clear();
+
+protected:
+	virtual void showEvent(QShowEvent * event);
+
+private Q_SLOTS:
+	void signalVisibility();
+	void selectAll(bool);
+
+private:
+	void updateCheckBoxes();
+
+Q_SIGNALS:
+	void visibilityChanged(int id, bool visible);
+
+private:
+	std::map<int, Transform> _poses;
+	std::map<int, bool> _mask;
+};
+
+} /* namespace rtabmap */
+#endif /* MAPVISIBILITYWIDGET_H_ */

rtabmap/guilib/include/rtabmap/gui/MultiSessionLocSubView.h

@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_MULTISESSIONLOCSUBVIEW_H_
+#define RTABMAP_MULTISESSIONLOCSUBVIEW_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+#include <opencv2/core.hpp>
+#include <QWidget>
+
+class Ui_multiSessionLocSubView;
+
+namespace rtabmap {
+
+class ImageView;
+
+class RTABMAP_GUI_EXPORT MultiSessionLocSubView : public QWidget
+{
+	Q_OBJECT
+
+public:
+	MultiSessionLocSubView(ImageView * mainView, int mapId, QWidget * parent = 0);
+	virtual ~MultiSessionLocSubView();
+
+public Q_SLOTS:
+	void updateView(
+			int nodeId,
+			const QImage & image,
+			const std::multimap<int, cv::KeyPoint> & features,
+			float locRatio,
+			const QColor & bgColor);
+	void clear();
+
+private:
+	Ui_multiSessionLocSubView * ui_;
+	int mapId_;
+};
+
+} /* namespace rtabmap */
+#endif /* RTABMAP_MULTISESSIONLOCSUBVIEW_H_ */

rtabmap/guilib/include/rtabmap/gui/MultiSessionLocWidget.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_MULTISESSIONLOCWIDGET_H_
+#define RTABMAP_MULTISESSIONLOCWIDGET_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Signature.h"
+#include <QWidget>
+
+class QPushButton;
+class QProgressBar;
+
+namespace rtabmap {
+
+class MultiSessionLocSubView;
+class ImageView;
+
+class RTABMAP_GUI_EXPORT MultiSessionLocWidget : public QWidget
+{
+	Q_OBJECT
+
+public:
+	MultiSessionLocWidget(
+			const QMap<int, Signature> * cache,
+			const std::map<int, int> * mapIds,
+			QWidget * parent = 0);
+	virtual ~MultiSessionLocWidget();
+
+	ImageView * getImageView() {return imageView_;}
+
+public Q_SLOTS:
+	void updateView(
+			const Signature & lastSignature,
+			const Statistics & stats);
+	void clear();
+
+private:
+	const QMap<int, Signature> * cache_;
+	const std::map<int, int> * mapIds_;
+	std::map<int, std::pair<MultiSessionLocSubView*, int> > subViews_;
+	ImageView * imageView_;
+	QProgressBar * totalLocProgressBar_;
+	QPushButton * resetbutton_;
+	int totalFrames_;
+	int totalLoops_;
+};
+
+} /* namespace rtabmap */
+#endif /* RTABMAP_MULTISESSIONLOCWIDGET_H_ */

rtabmap/guilib/include/rtabmap/gui/OdometryViewer.h

@@ -0,0 +1,98 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_ODOMETRYVIEWER_H_
+#define RTABMAP_ODOMETRYVIEWER_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Parameters.h"
+#include <QDialog>
+#include "rtabmap/utilite/UEventsHandler.h"
+
+class QSpinBox;
+class QDoubleSpinBox;
+class QLabel;
+class QCheckBox;
+
+namespace rtabmap {
+
+class ImageView;
+class CloudViewer;
+
+class RTABMAP_GUI_EXPORT OdometryViewer : public QDialog, public UEventsHandler
+{
+	Q_OBJECT
+
+public:
+	OdometryViewer(
+		int maxClouds = 10, 
+		int decimation = 2, 
+		float voxelSize = 0.0f, 
+		float maxDepth = 0, 
+		int qualityWarningThr=0, 
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
+	virtual ~OdometryViewer();
+
+public Q_SLOTS:
+	virtual void clear();
+
+protected:
+	virtual bool handleEvent(UEvent * event);
+
+private Q_SLOTS:
+	void reset();
+	void processData(const rtabmap::OdometryEvent & odom);
+
+private:
+	ImageView* imageView_;
+	CloudViewer* cloudView_;
+	bool processingData_;
+	bool odomImageShow_;
+	bool odomImageDepthShow_;
+
+	Transform lastOdomPose_;
+	int qualityWarningThr_;
+	int id_;
+	QList<std::string> addedClouds_;
+
+	QSpinBox * maxCloudsSpin_;
+	QDoubleSpinBox * voxelSpin_;
+	QSpinBox * decimationSpin_;
+	QDoubleSpinBox * maxDepthSpin_;
+	QCheckBox * cloudShown_;
+	QCheckBox * scanShown_;
+	QCheckBox * featuresShown_;
+	QLabel * timeLabel_;
+	int validDecimationValue_;
+	ParametersMap parameters_;
+};
+
+} /* namespace rtabmap */
+#endif /* ODOMETRYVIEWER_H_ */

rtabmap/guilib/include/rtabmap/gui/ParametersToolBox.h

@@ -0,0 +1,90 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+//
+// Original version from Find-Object: https://github.com/introlab/find-object
+//
+
+#ifndef RTABMAP_PARAMETERSTOOLBOX_H_
+#define RTABMAP_PARAMETERSTOOLBOX_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <QWidget>
+#include <QSet>
+
+class QVBoxLayout;
+class QStackedWidget;
+class QComboBox;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT ParametersToolBox: public QWidget
+{
+	Q_OBJECT
+
+public:
+	ParametersToolBox(QWidget *parent = 0);
+	virtual ~ParametersToolBox();
+
+	void setupUi(const ParametersMap & parameters);
+	QWidget * getParameterWidget(const QString & key);
+	void updateParameter(const std::string & key, const std::string & value);
+	const ParametersMap & getParameters() const {return parameters_;}
+
+private:
+	void addParameter(QVBoxLayout * layout, const std::string & key, const std::string & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const QString & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const int & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const double & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const bool & value);
+	void addParameter(QVBoxLayout * layout, const QString & name, QWidget * widget);
+
+Q_SIGNALS:
+	void parametersChanged(const QStringList & name);
+
+private Q_SLOTS:
+	void changeParameter();
+	void changeParameter(const QString & value);
+	void changeParameter(const int & value);
+	void resetCurrentPage();
+	void resetAllPages();
+
+private:
+	QStringList resetPage(int index);
+	void updateParametersVisibility();
+
+private:
+	QComboBox * comboBox_;
+	QStackedWidget * stackedWidget_;
+	ParametersMap parameters_;
+};
+
+} // namespace find_object
+
+#endif /* PARAMETERSTOOLBOX_H_ */

rtabmap/guilib/include/rtabmap/gui/PdfPlot.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_PDFPLOT_H_
+#define RTABMAP_PDFPLOT_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UPlot.h>
+#include "opencv2/opencv.hpp"
+#include "rtabmap/core/Signature.h"
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT PdfPlotItem : public UPlotItem
+{
+public:
+	PdfPlotItem(float dataX, float dataY, float width, int childCount = -1);
+	virtual ~PdfPlotItem();
+
+	void setLikelihood(int id, float value, int childCount);
+	void setSignaturesRef(const QMap<int, Signature> * signaturesRef) {_signaturesRef = signaturesRef;}
+
+	float value() const {return this->data().y();}
+	int id() const {return this->data().x();}
+
+protected:
+	virtual void showDescription(bool shown);
+
+private:
+	QGraphicsPixmapItem * _img;
+	int _childCount;
+	const QMap<int, Signature> * _signaturesRef;
+	QGraphicsTextItem * _text;
+
+};
+
+class RTABMAP_GUI_EXPORT PdfPlotCurve : public UPlotCurve
+{
+	Q_OBJECT
+
+public:
+	PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef, QObject * parent = 0);
+	virtual ~PdfPlotCurve();
+
+	virtual void clear();
+	void setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap);
+
+private:
+	const QMap<int, Signature> * _signaturesMapRef;
+};
+
+}
+
+#endif /* PDFPLOT_H_ */

rtabmap/guilib/include/rtabmap/gui/PostProcessingDialog.h

@@ -0,0 +1,105 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_POSTPROCESSINGDIALOG_H_
+#define RTABMAP_POSTPROCESSINGDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QtCore/QSettings>
+
+#include <rtabmap/core/Optimizer.h>
+
+class Ui_PostProcessingDialog;
+class QAbstractButton;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT PostProcessingDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	PostProcessingDialog(QWidget * parent = 0);
+
+	virtual ~PostProcessingDialog();
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	//getters
+	bool isDetectMoreLoopClosures() const;
+	double clusterRadius() const;
+	double clusterAngle() const;
+	int iterations() const;
+	bool intraSession() const;
+	bool interSession() const;
+	bool isRefineNeighborLinks() const;
+	bool isRefineLoopClosureLinks() const;
+	bool isSBA() const;
+	int sbaIterations() const;
+	double sbaVariance() const;
+	Optimizer::Type sbaType() const;
+	bool sbaRematchFeatures() const;
+
+	//setters
+	void setDetectMoreLoopClosures(bool on);
+	void setClusterRadius(double radius);
+	void setClusterAngle(double angle);
+	void setIterations(int iterations);
+	void setIntraSession(bool enabled);
+	void setInterSession(bool enabled);
+	void setRefineNeighborLinks(bool on);
+	void setRefineLoopClosureLinks(bool on);
+	void setSBA(bool on);
+	void setSBAIterations(int iterations);
+	void setSBAVariance(double variance);
+	void setSBAType(Optimizer::Type type);
+	void setSBARematchFeatures(bool value);
+
+Q_SIGNALS:
+	void configChanged();
+
+public Q_SLOTS:
+	void closeDialog ( QAbstractButton * button );
+	void restoreDefaults();
+
+private Q_SLOTS:
+	void updateVisibility();
+	void updateButtonBox();
+
+private:
+	bool validateForm();
+
+private:
+	Ui_PostProcessingDialog * _ui;
+};
+
+}
+
+#endif /* POSTPROCESSINGDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -0,0 +1,493 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_PREFERENCESDIALOG_H_
+#define RTABMAP_PREFERENCESDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+#include <QtCore/QModelIndex>
+#include <QtCore/QVector>
+#include <set>
+
+#include "rtabmap/core/Transform.h"
+#include "rtabmap/core/Parameters.h"
+
+class Ui_preferencesDialog;
+class QAbstractItemModel;
+class QAbstractButton;
+class QStandardItemModel;
+class QStandardItem;
+class QFile;
+class QGroupBox;
+class QMainWindow;
+class QLineEdit;
+class QSlider;
+class QProgressDialog;
+class UPlotCurve;
+class QStackedWidget;
+class QCheckBox;
+class QSpinBox;
+class QDoubleSpinBox;
+
+namespace rtabmap {
+
+class Signature;
+class LoopClosureViewer;
+class Camera;
+class SensorCapture;
+class Lidar;
+class CalibrationDialog;
+class CreateSimpleCalibrationDialog;
+
+class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	enum PanelFlag {
+		kPanelDummy = 0,
+		kPanelGeneral = 1,
+		kPanelCloudRendering = 2,
+		kPanelLogging = 4,
+		kPanelSource = 8,
+		kPanelCalibration = 16,
+		kPanelAll = 31
+	};
+	// TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared...
+	Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
+
+	enum Src {
+		kSrcUndef = -1,
+
+		kSrcRGBD           = 0,
+		kSrcOpenNI_PCL     = 0,
+		kSrcFreenect       = 1,
+		kSrcOpenNI_CV      = 2,
+		kSrcOpenNI_CV_ASUS = 3,
+		kSrcOpenNI2        = 4,
+		kSrcFreenect2      = 5,
+		kSrcRealSense      = 6,
+		kSrcRGBDImages     = 7,
+		kSrcK4W2           = 8,
+		kSrcRealSense2     = 9,
+		kSrcK4A            = 10,
+		kSrcSeerSense      = 11,
+
+		kSrcStereo         = 100,
+		kSrcDC1394         = 100,
+		kSrcFlyCapture2    = 101,
+		kSrcStereoImages   = 102,
+		kSrcStereoVideo    = 103,
+		kSrcStereoZed      = 104,
+		kSrcStereoUsb      = 105,
+		kSrcStereoTara 	   = 106,
+		kSrcStereoRealSense2 = 107,
+		kSrcStereoMyntEye  = 108,
+		kSrcStereoZedOC    = 109,
+		kSrcStereoDepthAI  = 110,
+
+		kSrcRGB            = 200,
+		kSrcUsbDevice      = 200,
+		kSrcImages         = 201,
+		kSrcVideo          = 202,
+
+		kSrcDatabase       = 300,
+
+		kSrcLidar           = 400,
+		kSrcLidarVLP16      = 400,
+	};
+
+public:
+	PreferencesDialog(QWidget * parent = 0);
+	virtual ~PreferencesDialog();
+
+	virtual QString getIniFilePath() const;
+	virtual QString getTmpIniFilePath() const;
+	void init(const QString & iniFilePath = "");
+	void setCurrentPanelToSource();
+	virtual QString getDefaultWorkingDirectory() const;
+
+	// save stuff
+	void saveSettings();
+	void saveWindowGeometry(const QWidget * window);
+	void loadWindowGeometry(QWidget * window);
+	void saveMainWindowState(const QMainWindow * mainWindow);
+	void loadMainWindowState(QMainWindow * mainWindow, bool & maximized, bool & statusBarShown);
+	void saveWidgetState(const QWidget * widget);
+	void loadWidgetState(QWidget * widget);
+
+	void saveCustomConfig(const QString & section, const QString & key, const QString & value);
+	QString loadCustomConfig(const QString & section, const QString & key);
+
+	rtabmap::ParametersMap getAllParameters() const;
+	std::string getParameter(const std::string & key) const;
+	void updateParameters(const ParametersMap & parameters, bool setOtherParametersToDefault = false);
+
+	//General panel
+	int getGeneralLoggerLevel() const;
+	int getGeneralLoggerEventLevel() const;
+	int getGeneralLoggerPauseLevel() const;
+	int getGeneralLoggerType() const;
+	bool getGeneralLoggerPrintTime() const;
+	bool getGeneralLoggerPrintThreadId() const;
+	std::vector<std::string> getGeneralLoggerThreads() const;
+	bool isVerticalLayoutUsed() const;
+	bool imageRejectedShown() const;
+	bool imageHighestHypShown() const;
+	bool beepOnPause() const;
+	bool isTimeUsedInFigures() const;
+	bool isCacheSavedInFigures() const;
+	bool notifyWhenNewGlobalPathIsReceived() const;
+	int getOdomQualityWarnThr() const;
+	bool isOdomOnlyInliersShown() const;
+	bool isPosteriorGraphView() const;
+	bool isWordsCountGraphView() const;
+	bool isLocalizationsCountGraphView() const;
+	bool isRelocalizationColorOdomCacheGraphView() const;
+	int getOdomRegistrationApproach() const;
+	double getOdomF2MGravitySigma() const;
+	bool isOdomDisabled() const;
+	bool isOdomSensorAsGt() const;
+	bool isGroundTruthAligned() const;
+
+	bool isGraphsShown() const;
+	bool isLabelsShown() const;
+	bool isFramesShown() const;
+	bool isLandmarksShown() const;
+	double landmarkVisSize() const;
+	bool isIMUGravityShown(int index) const;
+	double getIMUGravityLength(int index) const;
+	bool isIMUAccShown() const;
+	bool isMarkerDetection() const;
+	double getMarkerLength() const;
+	double getVoxel() const;
+	double getNoiseRadius() const;
+	int getNoiseMinNeighbors() const;
+	double getCeilingFilteringHeight() const;
+	double getFloorFilteringHeight() const;
+	int getNormalKSearch() const;
+	double getNormalRadiusSearch() const;
+	double getScanCeilingFilteringHeight() const;
+	double getScanFloorFilteringHeight() const;
+	int getScanNormalKSearch() const;
+	double getScanNormalRadiusSearch() const;
+	bool isCloudsShown(int index) const;      // 0=map, 1=odom
+	bool isOctomapUpdated() const;
+	bool isOctomapShown() const;
+	int getOctomapRenderingType() const;
+	bool isOctomap2dGrid() const;
+	int getOctomapTreeDepth() const;
+	int getOctomapPointSize() const;
+	int getCloudDecimation(int index) const;   // 0=map, 1=odom
+	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom
+	double getCloudMinDepth(int index) const;  // 0=map, 1=odom
+	std::vector<float> getCloudRoiRatios(int index) const; // 0=map, 1=odom
+	unsigned char getCloudConfidenceThr(int index) const; // 0=map, 1=odom
+	int getCloudColorScheme(int index) const;   // 0=map, 1=odom
+	double getCloudOpacity(int index) const;   // 0=map, 1=odom
+	int getCloudPointSize(int index) const;    // 0=map, 1=odom
+
+	bool isScansShown(int index) const;       // 0=map, 1=odom
+	int getDownsamplingStepScan(int index) const; // 0=map, 1=odom
+	double getScanMaxRange(int index) const; // 0=map, 1=odom
+	double getScanMinRange(int index) const; // 0=map, 1=odom
+	double getCloudVoxelSizeScan(int index) const; // 0=map, 1=odom
+	int getScanColorScheme(int index) const;    // 0=map, 1=odom
+	double getScanOpacity(int index) const;    // 0=map, 1=odom
+	int getScanPointSize(int index) const;     // 0=map, 1=odom
+
+	bool isFeaturesShown(int index) const;     // 0=map, 1=odom
+	bool isFrustumsShown(int index) const;     // 0=map, 1=odom
+	int getFeaturesPointSize(int index) const; // 0=map, 1=odom
+
+	bool isCloudFiltering() const;
+	bool isSubtractFiltering() const;
+	double getCloudFilteringRadius() const;
+	double getCloudFilteringAngle() const;
+	int getSubtractFilteringMinPts() const;
+	double getSubtractFilteringRadius() const;
+	double getSubtractFilteringAngle() const;
+
+	double getGridUIResolution() const;
+	bool getGridMapShown() const;
+	int getElevationMapShown() const;
+	int getGridMapSensor() const;
+	bool projMapFrame() const;
+	double projMaxGroundAngle() const;
+	double projMaxGroundHeight() const;
+	int projMinClusterSize() const;
+	double projMaxObstaclesHeight() const;
+	bool projFlatObstaclesDetected() const;
+	double getGridMapOpacity() const;
+
+	bool isCloudMeshing() const;
+	double getCloudMeshingAngle() const;
+	bool isCloudMeshingQuad() const;
+	bool isCloudMeshingTexture() const;
+	int getCloudMeshingTriangleSize();
+
+	QString getWorkingDirectory() const;
+
+	// source panel
+	double getGeneralInputRate() const;
+	bool isSourceMirroring() const;
+	PreferencesDialog::Src getSourceType() const;
+	PreferencesDialog::Src getSourceDriver() const;
+	QString getSourceDriverStr() const;
+	QString getSourceDevice() const;
+	PreferencesDialog::Src getLidarSourceDriver() const;
+	PreferencesDialog::Src getOdomSourceDriver() const;
+
+	bool isSourceDatabaseStampsUsed() const;
+	bool isSourceDatabaseStereoToDepth() const;
+	bool isSourceRGBDColorOnly() const;
+	int getIMUFilteringStrategy() const;
+	bool getIMUFilteringBaseFrameConversion() const;
+	bool isDepthFilteringAvailable() const;
+	QString getSourceDistortionModel() const;
+	bool isBilateralFiltering() const;
+	double getBilateralSigmaS() const;
+	double getBilateralSigmaR() const;
+	int getSourceImageDecimation() const;
+	int getSourceHistogramMethod() const;
+	bool isSourceFeatureDetection() const;
+	bool isSourceStereoDepthGenerated() const;
+	bool isSourceStereoExposureCompensation() const;
+	bool isRightGrayScale() const;
+	bool isSourceScanFromDepth() const;
+	bool isSourceScanDeskewing() const;
+	int getSourceScanDownsampleStep() const;
+	double getSourceScanRangeMin() const;
+	double getSourceScanRangeMax() const;
+	double getSourceScanVoxelSize() const;
+	int getSourceScanNormalsK() const;
+	double getSourceScanNormalsRadius() const;
+	double getSourceScanForceGroundNormalsUp() const;
+	Transform getSourceLocalTransform() const;    //Openni group
+	Transform getLaserLocalTransform() const; // directory images
+	Transform getIMULocalTransform() const; // directory images
+	QString getIMUPath() const;
+	int getIMURate() const;
+	Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null
+	SensorCapture * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor, double & waitTime); // return odom sensor should be deleted if not null
+	Lidar * createLidar(); // return lidar should be deleted if not null
+
+	int getIgnoredDCComponents() const;
+
+	//
+	bool isImagesKept() const;
+	bool isMissingCacheRepublished() const;
+	bool isCloudsKept() const;
+	float getTimeLimit() const;
+	float getDetectionRate() const;
+	bool isSLAMMode() const;
+	bool isRGBDMode() const;
+	int getKpMaxFeatures() const;
+	bool isPriorIgnored() const;
+
+	//specific
+	bool isStatisticsPublished() const;
+	double getLoopThr() const;
+	double getVpThr() const;
+	double getSimThr() const;
+	int getOdomStrategy() const;
+	int getOdomBufferSize() const;
+	QString getCameraInfoDir() const; // "workinfDir/camera_info"
+
+	//
+	void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}
+
+Q_SIGNALS:
+	void settingsChanged(PreferencesDialog::PANEL_FLAGS);
+	void settingsChanged(rtabmap::ParametersMap);
+
+public Q_SLOTS:
+	void setInputRate(double value);
+	void setDetectionRate(double value);
+	void setTimeLimit(float value);
+	void setSLAMMode(bool enabled);
+	void selectSourceDriver(Src src, int variant = 0);
+	void calibrate();
+	void calibrateSimple();
+	void calibrateOdomSensorExtrinsics();
+	void testLidar();
+
+private Q_SLOTS:
+	void closeDialog ( QAbstractButton * button );
+	void resetApply ( QAbstractButton * button );
+	void resetSettings(int panelNumber);
+	void loadConfigFrom();
+	bool saveConfigTo();
+	void resetConfig();
+	void loadPreset();
+	void makeObsoleteGeneralPanel();
+	void makeObsoleteCloudRenderingPanel();
+	void makeObsoleteLoggingPanel();
+	void makeObsoleteSourcePanel();
+	void makeObsoleteCalibrationPanel();
+	void clicked(const QModelIndex & current, const QModelIndex & previous);
+	void addParameter(int value);
+	void addParameter(bool value);
+	void addParameter(double value);
+	void addParameter(const QString & value);
+	void updatePredictionPlot();
+	void updateKpROI();
+	void updateStereoDisparityVisibility();
+	void updateFeatureMatchingVisibility();
+	void updateGlobalDescriptorVisibility();
+	void updateOdometryStackedIndex(int index);
+	void useOdomFeatures();
+	void changeWorkingDirectory();
+	void changeDictionaryPath();
+	void changeOdometryORBSLAMVocabulary();
+	void changeOdometryOKVISConfigPath();
+	void changeOdometryVINSConfigPath();
+	void changeOdometryOpenVINSLeftMask();
+	void changeOdometryOpenVINSRightMask();
+	void changeIcpPMConfigPath();
+	void changeSuperPointModelPath();
+	void changePyMatcherPath();
+	void changePyMatcherModel();
+	void changePyDescriptorPath();
+	void changePyDetectorPath();
+	void readSettingsEnd();
+	void setupTreeView();
+	void updateBasicParameter();
+	void openDatabaseViewer();
+	void visualizeDistortionModel();
+	void selectSourceDatabase();
+	void selectCalibrationPath();
+	void selectOdomSensorCalibrationPath();
+	void selectSourceImagesStamps();
+	void selectSourceRGBDImagesPathRGB();
+	void selectSourceRGBDImagesPathDepth();
+	void selectSourceImagesPathScans();
+	void selectSourceImagesPathIMU();
+	void selectSourceImagesPathOdom();
+	void selectSourceImagesPathGt();
+	void selectSourceStereoImagesPathLeft();
+	void selectSourceStereoImagesPathRight();
+	void selectSourceImagesPath();
+	void selectSourceVideoPath();
+	void selectSourceStereoVideoPath();
+	void selectSourceStereoVideoPath2();
+	void selectSourceDistortionModel();
+	void selectSourceOniPath();
+	void selectSourceOni2Path();
+	void selectSourceMKVPath();
+	void selectSourceSvoPath();
+	void selectSourceRealsense2JsonPath();
+	void selectSourceDepthaiBlobPath();
+	void selectVlp16PcapPath();
+	void updateSourceGrpVisibility();
+	void testOdometry();
+	void testCamera();
+
+protected:
+	virtual void showEvent ( QShowEvent * event );
+	virtual void closeEvent(QCloseEvent *event);
+
+	virtual QString getParamMessage();
+
+	virtual void readGuiSettings(const QString & filePath = QString());
+	virtual void readCameraSettings(const QString & filePath = QString());
+	virtual bool readCoreSettings(const QString & filePath = QString());
+
+	virtual void writeGuiSettings(const QString & filePath = QString()) const;
+	virtual void writeCameraSettings(const QString & filePath = QString()) const;
+	virtual void writeCoreSettings(const QString & filePath = QString()) const;
+
+	void setParameter(const std::string & key, const std::string & value);
+
+private:
+	void readSettings(const QString & filePath = QString());
+	void writeSettings(const QString & filePath = QString());
+	bool validateForm();
+	void setupSignals();
+	void setupKpRoiPanel();
+	bool parseModel(QList<QGroupBox*> & boxes, QStandardItem * parentItem, int currentLevel, int & absoluteIndex);
+	void resetSettings(QGroupBox * groupBox);
+	void loadPreset(const std::string & presetHexHeader);
+	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, int panel = -1);
+	void addParameters(const QGroupBox * box);
+	QList<QGroupBox*> getGroupBoxes();
+	void readSettingsBegin();
+	Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly, bool odomSensorExtrinsicsCalib); // return camera should be deleted if not null
+
+protected:
+	PANEL_FLAGS _obsoletePanels;
+
+private:
+	rtabmap::ParametersMap _modifiedParameters;
+	rtabmap::ParametersMap _parameters;
+	Ui_preferencesDialog * _ui;
+	QStandardItemModel * _indexModel;
+	bool _initialized;
+	bool _monitoringState;
+
+	QProgressDialog * _progressDialog;
+
+	//calibration
+	CalibrationDialog * _calibrationDialog;
+	CreateSimpleCalibrationDialog * _createCalibrationDialog;
+
+	QVector<QCheckBox*> _3dRenderingShowClouds;
+	QVector<QSpinBox*> _3dRenderingDecimation;
+	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
+	QVector<QDoubleSpinBox*> _3dRenderingMinDepth;
+	QVector<QLineEdit*> _3dRenderingRoiRatios;
+	QVector<QSpinBox*> _3dRenderingDepthConfidenceThr;
+	QVector<QSpinBox*> _3dRenderingColorScheme;
+	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
+	QVector<QSpinBox*> _3dRenderingPtSize;
+	QVector<QCheckBox*> _3dRenderingShowScans;
+	QVector<QSpinBox*> _3dRenderingDownsamplingScan;
+	QVector<QDoubleSpinBox*> _3dRenderingMaxRange;
+	QVector<QDoubleSpinBox*> _3dRenderingMinRange;
+	QVector<QDoubleSpinBox*> _3dRenderingVoxelSizeScan;
+	QVector<QSpinBox*> _3dRenderingColorSchemeScan;
+	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
+	QVector<QSpinBox*> _3dRenderingPtSizeScan;
+	QVector<QCheckBox*> _3dRenderingShowFeatures;
+	QVector<QCheckBox*> _3dRenderingShowFrustums;
+	QVector<QSpinBox*> _3dRenderingPtSizeFeatures;
+	QVector<QCheckBox*> _3dRenderingGravity;
+	QVector<QDoubleSpinBox*> _3dRenderingGravityLength;
+};
+
+Q_DECLARE_OPERATORS_FOR_FLAGS(PreferencesDialog::PANEL_FLAGS)
+
+}
+
+#endif /* PREFERENCESDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/ProgressDialog.h

@@ -0,0 +1,89 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_PROGRESSDIALOG_H_
+#define RTABMAP_PROGRESSDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QDialog>
+
+class QLabel;
+class QTextEdit;
+class QProgressBar;
+class QPushButton;
+class QCheckBox;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT ProgressDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	ProgressDialog(QWidget *parent = 0, Qt::WindowFlags flags = Qt::WindowFlags());
+	virtual ~ProgressDialog();
+
+	void setEndMessage(const QString & message) {_endMessage = message;} // Message shown when the progress is finished
+	void setValue(int value);
+	int maximumSteps() const;
+	void setMaximumSteps(int steps);
+	void setAutoClose(bool on, int delayedClosingTimeSec = -1);
+	void setCancelButtonVisible(bool visible);
+	bool isCanceled() const {return _canceled;}
+
+Q_SIGNALS:
+	void canceled();
+
+protected:
+	virtual void closeEvent(QCloseEvent * event);
+
+public Q_SLOTS:
+	void appendText(const QString & text ,const QColor & color = Qt::black);
+	void incrementStep(int steps = 1);
+	void clear();
+	void resetProgress();
+
+private Q_SLOTS:
+	void closeDialog();
+	void cancel();
+
+private:
+	QLabel * _text;
+	QTextEdit * _detailedText;
+	QProgressBar * _progressBar;
+	QPushButton * _closeButton;
+	QPushButton * _cancelButton;
+	QCheckBox * _closeWhenDoneCheckBox;
+	QString _endMessage;
+	int _delayedClosingTime; // sec
+	bool _canceled;
+};
+
+}
+
+#endif /* PROGRESSDIALOG_H_ */

rtabmap/guilib/include/rtabmap/gui/RecoveryState.h

@@ -0,0 +1,103 @@
+/*
+Copyright (c) 2010-2017, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_RECOVERYSTATE_H_
+#define RTABMAP_RECOVERYSTATE_H_
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include "rtabmap/core/ProgressState.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <QApplication>
+
+namespace rtabmap {
+
+class RecoveryState : public QObject, public ProgressState
+{
+	Q_OBJECT
+
+public:
+	RecoveryState(ProgressDialog * dialog): dialog_(dialog)
+	{
+		connect(dialog_, SIGNAL(canceled()), this, SLOT(cancel()));
+	}
+	virtual ~RecoveryState() {}
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+		{
+			QString msgQt = msg.c_str();
+			if(msgQt.contains("Processed"))
+			{
+				dialog_->incrementStep();
+				dialog_->appendText(msg.c_str());
+			}
+			else if(msgQt.contains("Found"))
+			{
+				std::list<std::string> strSplit = uSplitNumChar(msg);
+				if(strSplit.size() == 3)
+				{
+					int nodes = uStr2Int(*(++strSplit.begin()));
+					if(nodes > 0)
+					{
+						dialog_->setMaximumSteps(nodes);
+					}
+				}
+				dialog_->appendText(msg.c_str());
+			}
+			else if(msgQt.contains("Skipping") ||
+					msgQt.contains("Failed processing"))
+			{
+				dialog_->appendText(msg.c_str(), Qt::darkYellow);
+			}
+			else
+			{
+				dialog_->appendText(msg.c_str());
+			}
+		}
+		QApplication::processEvents();
+		if(!isCanceled())
+		{
+			return ProgressState::callback(msg);
+		}
+		return false;
+	}
+
+public Q_SLOTS:
+	void cancel()
+	{
+		setCanceled(true);
+	}
+
+private:
+	ProgressDialog * dialog_;
+};
+
+}
+
+
+#endif /* RECOVERYSTATE_H_ */

rtabmap/guilib/include/rtabmap/gui/StatsToolBox.h

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_STATSTOOLBOX_H_
+#define RTABMAP_STATSTOOLBOX_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QWidget>
+#include <QtCore/QMap>
+
+class QToolButton;
+class QLabel;
+class QMenu;
+class QGridLayout;
+class QToolBox;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT StatItem : public QWidget
+{
+	Q_OBJECT;
+
+public:
+	StatItem(const QString & name, bool cacheOn, const std::vector<qreal> & x, const std::vector<qreal> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
+	virtual ~StatItem();
+	void addValue(qreal y);
+	void addValue(qreal x, qreal y);
+	void setValues(const std::vector<qreal> & x, const std::vector<qreal> & y);
+	QString value() const;
+	std::vector<qreal> xValues() const {return _x;}
+	std::vector<qreal> yValues() const {return _y;}
+	void setCacheOn(bool on);
+	void clearCache();
+
+public Q_SLOTS:
+	void updateMenu(const QMenu * menu);
+
+Q_SIGNALS:
+	void valueAdded(qreal);
+	void valueAdded(qreal, qreal);
+	void valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &);
+	void plotRequested(const StatItem *, const QString &);
+
+private Q_SLOTS:
+	void preparePlotRequest();
+
+private:
+	void setupUi(QGridLayout * grid);
+
+private:
+	QToolButton * _button;
+	QLabel * _name;
+	QLabel * _value;
+	QLabel * _unit;
+	QMenu * _menu;
+
+	bool _cacheOn;
+	std::vector<qreal> _x;
+	std::vector<qreal> _y;
+};
+
+
+
+
+class RTABMAP_GUI_EXPORT StatsToolBox : public QWidget
+{
+	Q_OBJECT;
+
+public:
+	StatsToolBox(QWidget * parent);
+	virtual ~StatsToolBox();
+
+	void getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds);
+	void addCurve(const QString & name, bool newFigure = true, bool cacheOn = false);
+	void addThreshold(const QString & name, qreal value); // Add to latest figure
+	void setWorkingDirectory(const QString & workingDirectory);
+	void setNewFigureMaxItems(int value) {_newFigureMaxItems = value;}
+	void closeFigures();
+	void setCacheOn(bool on);
+
+public Q_SLOTS:
+	void updateStat(const QString & statFullName, bool cacheOn);
+	void updateStat(const QString & statFullName, qreal y, bool cacheOn);
+	void updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn);
+	void updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn);
+	void clear();
+
+Q_SIGNALS:
+	void menuChanged(const QMenu *);
+	void figuresSetupChanged();
+
+private Q_SLOTS:
+	void plot(const StatItem * stat, const QString & plotName = QString());
+	void figureDeleted(QObject * obj);
+
+protected:
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+
+private:
+	QMenu * _plotMenu;
+	QToolBox * _statBox;
+	QString _workingDirectory;
+	int _newFigureMaxItems;
+	QMap<QString, QWidget*> _figures;
+};
+
+}
+
+#endif /* STATSTOOLBOX_H_ */

rtabmap/guilib/include/rtabmap/gui/TexturingState.h

@@ -0,0 +1,80 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#ifndef RTABMAP_TEXTURINGSTATE_H_
+#define RTABMAP_TEXTURINGSTATE_H_
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include "rtabmap/core/ProgressState.h"
+#include <QApplication>
+
+namespace rtabmap {
+
+class TexturingState : public QObject, public ProgressState
+{
+	Q_OBJECT
+
+public:
+	TexturingState(ProgressDialog * dialog, bool incrementOnMsgReceived): dialog_(dialog)
+	{
+		_increment = incrementOnMsgReceived;
+		connect(dialog_, SIGNAL(canceled()), this, SLOT(cancel()));
+	}
+	virtual ~TexturingState() {}
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+		{
+			dialog_->appendText(msg.c_str());
+			if(_increment)
+			{
+				dialog_->incrementStep();
+			}
+		}
+		QApplication::processEvents();
+		if(!isCanceled())
+		{
+			return ProgressState::callback(msg);
+		}
+		return false;
+	}
+
+public Q_SLOTS:
+	void cancel()
+	{
+		setCanceled(true);
+	}
+
+private:
+	ProgressDialog * dialog_;
+	bool _increment;
+};
+
+}
+
+#endif /* GUILIB_SRC_TEXTURINGSTATE_H_ */

rtabmap/guilib/include/rtabmap/utilite/UCv2Qt.h

@@ -0,0 +1,303 @@
+/*
+*  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 <QtGui/QColor>
+#include <opencv2/core/core.hpp>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UThread.h>
+#include <stdio.h>
+
+enum uCvQtDepthColorMap{
+	uCvQtDepthWhiteToBlack,
+	uCvQtDepthBlackToWhite,
+	uCvQtDepthRedToBlue,
+	uCvQtDepthBlueToRed
+};
+
+/**
+ * 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.
+ * @param colorMap gradient of color to use to visualize depth or monochrome images
+ * @param depthMin fixed minimum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
+ * @param depthMax fixed maximum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
+ * @return the QImage
+ */
+inline QImage uCvMat2QImage(
+		const cv::Mat & image,
+		bool isBgr = true,
+		uCvQtDepthColorMap colorMap = uCvQtDepthBlackToWhite,
+		float depthMin = 0,
+		float depthMax = 0)
+{
+	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();
+
+			QVector<QRgb> my_table;
+			my_table.reserve(256);
+			if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+			{
+				if(colorMap == uCvQtDepthBlueToRed) {
+					for(int i = 0; i < 256; i++)
+						my_table.push_back(QColor::fromHsv(255-i, 255, 255, 255).rgb());
+				}
+				else {
+					for(int i = 0; i < 256; i++)
+						my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+				}
+			}
+			else if(colorMap == uCvQtDepthBlackToWhite)
+			{
+				for(int i = 0; i < 256; i++)
+					my_table.push_back(qRgb(i,i,i));
+			}
+			else // uCvQtDepthWhiteToBlack
+			{
+				for(int i = 0; i < 256; i++)
+					my_table.push_back(qRgb(255-i,255-i,255-i));
+			}
+			qtemp.setColorTable(my_table);
+		}
+		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,max;
+		if(depthMax>depthMin)
+		{
+			min = depthMin;
+			max = depthMax;
+		}
+		else
+		{
+			min = max = data[0];
+			for(unsigned int i=1; i<image.total(); ++i)
+			{
+				if(uIsFinite(data[i]) && data[i] > 0)
+				{
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					{
+						min = data[i];
+					}
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					{
+						max = data[i];
+					}
+				}
+			}
+			if(depthMax > 0 && depthMax > depthMin)
+			{
+				max = depthMax;
+			}
+			if(depthMin>0 && (depthMin < depthMax || depthMin < max))
+			{
+				min = depthMin;
+			}
+		}
+
+		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(!uIsFinite(data[x]) || max == min || data[x] == 0)
+				{
+					*p = 0;
+				}
+				else if(data[x] < min)
+				{
+					*p = 255;
+				}
+				else if(data[x] > max)
+				{
+					*p=1;
+				}
+				else
+				{
+					*p = uchar(std::max(1.0f, std::min(255.0f, 255.0f - ((data[x]-min)*255.0f)/(max-min))));
+				}
+				if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue))
+				{
+					*p = 255-*p+1;
+				}
+			}
+		}
+
+		QVector<QRgb> my_table;
+		my_table.reserve(256);
+		if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+		{
+			my_table.push_back(qRgb(0,0,0));
+			for(int i = 1; i < 256; i++)
+				my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+		}
+		else
+		{
+			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,max;
+		if(depthMax>depthMin)
+		{
+			min = depthMin*1000;
+			max = depthMax*1000;
+		}
+		else
+		{
+			min = max = data[0];
+			for(unsigned int i=1; i<image.total(); ++i)
+			{
+				if(uIsFinite(data[i]) && data[i] > 0)
+				{
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					{
+						min = data[i];
+					}
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					{
+						max = data[i];
+					}
+				}
+			}
+			if(depthMax > 0 && depthMax > depthMin)
+			{
+				max = depthMax*1000;
+			}
+			if(depthMin>0 && (depthMin < depthMax || depthMin*1000 < max))
+			{
+				min = depthMin*1000;
+			}
+		}
+
+		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(!uIsFinite(data[x]) || max == min || data[x]==0)
+				{
+					*p = 0;
+				}
+				else if(data[x] < min)
+				{
+					*p = 255;
+				}
+				else if(data[x] > max)
+				{
+					*p = 1;
+				}
+				else
+				{
+					*p = uchar(std::max(1.0f, std::min(255.0f, 255.0f - (float(data[x]-min)/float(max-min))*255.0f)));
+				}
+				if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue))
+				{
+					*p = 255-*p+1;
+				}
+			}
+		}
+
+		QVector<QRgb> my_table;
+		my_table.reserve(256);
+		if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+		{
+			my_table.push_back(qRgb(0,0,0));
+			for(int i = 1; i < 256; i++)
+				my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+		}
+		else
+		{
+			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_ */

rtabmap/guilib/include/rtabmap/utilite/UImageView.h

@@ -0,0 +1,98 @@
+/*
+ * ImageView.h
+ *
+ *  Created on: 2012-06-20
+ *      Author: mathieu
+ */
+
+#ifndef IMAGEVIEW_H_
+#define IMAGEVIEW_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QWidget>
+#include <QtGui/QPainter>
+
+class RTABMAP_GUI_EXPORT UImageView : public QWidget
+{
+	Q_OBJECT;
+public:
+	UImageView(QWidget * parent = 0) : QWidget(parent) {}
+	~UImageView() {}
+	void setBackgroundBrush(const QBrush & brush) {brush_ = brush;}
+
+public Q_SLOTS:
+	void setImage(const QImage & image)
+	{
+		pixmap_ = QPixmap::fromImage(image);
+		this->update();
+	}
+
+private:
+	void computeScaleOffsets(float & scale, float & offsetX, float & offsetY)
+	{
+		scale = 1.0f;
+		offsetX = 0.0f;
+		offsetY = 0.0f;
+
+		if(!pixmap_.isNull())
+		{
+			float w = pixmap_.width();
+			float h = pixmap_.height();
+			float widthRatio = float(this->rect().width()) / w;
+			float heightRatio = float(this->rect().height()) / h;
+
+			if(widthRatio < heightRatio)
+			{
+				scale = widthRatio;
+			}
+			else
+			{
+				scale = heightRatio;
+			}
+
+			w *= scale;
+			h *= scale;
+
+			if(w < this->rect().width())
+			{
+				offsetX = (this->rect().width() - w)/2.0f;
+			}
+			if(h < this->rect().height())
+			{
+				offsetY = (this->rect().height() - h)/2.0f;
+			}
+		}
+	}
+
+protected:
+	virtual void paintEvent(QPaintEvent *event)
+	{
+		QPainter painter(this);
+
+		//Draw background
+		painter.save();
+		painter.setBrush(brush_);
+		painter.drawRect(this->rect());
+		painter.restore();
+
+		if(!pixmap_.isNull())
+		{
+			painter.save();
+			//Scale
+			float ratio, offsetX, offsetY;
+			this->computeScaleOffsets(ratio, offsetX, offsetY);
+			painter.translate(offsetX, offsetY);
+			painter.scale(ratio, ratio);
+			painter.drawPixmap(QPoint(0,0), pixmap_);
+			painter.restore();
+		}
+	}
+
+private:
+	QPixmap pixmap_;
+	QBrush brush_;
+};
+
+
+#endif /* IMAGEVIEW_H_ */

rtabmap/guilib/include/rtabmap/utilite/UPlot.h

@@ -0,0 +1,644 @@
+/*
+*  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 UPLOT_H_
+#define UPLOT_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <QFrame>
+#include <QtCore/QList>
+#include <QtCore/QMap>
+#include <QtGui/QPen>
+#include <QtGui/QBrush>
+#include <QGraphicsEllipseItem>
+#include <QtCore/QMutex>
+#include <QLabel>
+#include <QPushButton>
+#include <QtCore/QTime>
+#include <QtCore/QElapsedTimer>
+
+class QGraphicsView;
+class QGraphicsScene;
+class QGraphicsItem;
+class QFormLayout;
+class QScrollArea;
+
+/**
+ * UPlotItem is a QGraphicsEllipseItem and can be inherited to do custom behaviors
+ * on an hoverEnterEvent() for example.
+ */
+class RTABMAP_GUI_EXPORT UPlotItem : public QGraphicsEllipseItem
+{
+public:
+	/**
+	 * Constructor 1.
+	 */
+	UPlotItem(qreal dataX, qreal dataY, qreal width=2);
+	/**
+	 * Constructor 2.
+	 */
+	UPlotItem(const QPointF & data, qreal width=2);
+	virtual ~UPlotItem();
+
+public:
+	void setNextItem(UPlotItem * nextItem);
+	void setPreviousItem(UPlotItem * previousItem);
+	void setData(const QPointF & data);
+
+	UPlotItem * nextItem() const {return _nextItem;}
+	UPlotItem * previousItem() const {return _previousItem;};
+	const QPointF & data() const {return _data;}
+
+protected:
+	virtual void hoverEnterEvent(QGraphicsSceneHoverEvent * event);
+	virtual void hoverLeaveEvent(QGraphicsSceneHoverEvent * event);
+	virtual void focusInEvent(QFocusEvent * event);
+	virtual void focusOutEvent(QFocusEvent * event);
+	virtual void keyReleaseEvent(QKeyEvent * keyEvent);
+
+	virtual void showDescription(bool shown);
+private:
+	void init(qreal dataX, qreal dataY);
+
+private:
+	QPointF _data;
+	UPlotItem * _previousItem;
+	UPlotItem * _nextItem;
+	QGraphicsTextItem * _text;
+	QGraphicsRectItem * _textBackground;
+};
+
+class UPlot;
+
+/**
+ * UPlotCurve is a curve used to hold data shown in a UPlot.
+ */
+class RTABMAP_GUI_EXPORT UPlotCurve : public QObject
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor 1
+	 */
+	UPlotCurve(const QString & name, QObject * parent = 0);
+	/**
+	 * Constructor 2
+	 */
+	UPlotCurve(const QString & name, const QVector<UPlotItem *> data, QObject * parent = 0);
+	/**
+	 * Constructor 3
+	 */
+	UPlotCurve(const QString & name, const QVector<qreal> & x, const QVector<qreal> & y, QObject * parent = 0);
+	virtual ~UPlotCurve();
+
+	/**
+	 * Get pen.
+	 */
+	const QPen & pen() const {return _pen;}
+	/**
+	 * Get brush.
+	 */
+	const QBrush & brush() const {return _brush;}
+
+	/**
+	 * Set pen.
+	 */
+	void setPen(const QPen & pen);
+	/**
+	 * Set brush.
+	 */
+	void setBrush(const QBrush & brush);
+
+	void setItemsColor(const QColor & color);
+	QColor itemsColor() const  {return _itemsColor;}
+
+	/**
+	 * Get name.
+	 */
+	QString name() const {return _name;}
+	/**
+	 * Get the number of items in the curve (dot + line items).
+	 */
+	int itemsSize() const;
+	QPointF getItemData(int index);
+	bool isVisible() const {return _visible;}
+	void setData(QVector<UPlotItem*> & data); // take the ownership
+	void getData(QVector<qreal> & x, QVector<qreal> & y) const; // only call in Qt MainThread
+	void getData(QMap<qreal,qreal> & data) const; // only call in Qt MainThread
+	void draw(QPainter * painter, const QRect & limits);
+
+public Q_SLOTS:
+	/**
+	 *
+	 * Clear curve's values.
+	 */
+	virtual void clear();
+	/**
+	 *
+	 * Show or hide the curve.
+	 */
+    void setVisible(bool visible);
+    /**
+     *
+	 * Set increment of the x values (when auto-increment is used).
+	 */
+    void setXIncrement(qreal increment);
+    /**
+     *
+	 * Set starting x value (when auto-increment is used).
+	 */
+    void setXStart(qreal val);
+    /**
+     *
+	 * Add a single value, using a custom UPlotItem.
+	 */
+	void addValue(UPlotItem * data); // take the ownership
+	/**
+	 *
+	 * Add a single value y, x is auto-incremented by the increment set with setXIncrement().
+	 * @see setXStart()
+	 */
+	void addValue(qreal y);
+	/**
+	 *
+	 * Add a single value y at x.
+	 */
+	void addValue(qreal x, qreal y);
+	/**
+	 *
+	 * For convenience...
+	 * Add a single value y, x is auto-incremented by the increment set with setXIncrement().
+	 * @see setXStart()
+	 */
+	void addValue(const QString & y);
+	/**
+	 *
+	 * For convenience...
+	 * Add multiple values, using custom UPlotItem.
+	 */
+	void addValues(QVector<UPlotItem *> & data); // take the ownership
+	/**
+	 *
+	 * Add multiple values y at x. Vectors must have the same size.
+	 */
+	void addValues(const QVector<qreal> & xs, const QVector<qreal> & ys);
+	/**
+	 *
+	 * Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
+	 * @see setXStart()
+	 */
+	void addValues(const QVector<qreal> & ys);
+	void addValues(const QVector<int> & ys); // for convenience
+	/**
+	 *
+	 * Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
+	 * @see setXStart()
+	 */
+	void addValues(const std::vector<qreal> & ys); // for convenience
+	void addValues(const std::vector<int> & ys); // for convenience
+
+	void setData(const QVector<qreal> & x, const QVector<qreal> & y);
+	void setData(const std::vector<qreal> & x, const std::vector<qreal> & y);
+	void setData(const QVector<qreal> & y);
+	void setData(const std::vector<qreal> & y);
+
+Q_SIGNALS:
+	/**
+	 *
+	 *  emitted when data is changed.
+	 */
+	void dataChanged(const UPlotCurve *);
+
+protected:
+	friend class UPlot;
+	void attach(UPlot * plot);
+	void detach(UPlot * plot);
+	void updateMinMax();
+	const QVector<qreal> & getMinMax() const {return _minMax;}
+	int removeItem(int index);
+	void _addValue(UPlotItem * data);;
+	virtual bool isMinMaxValid() const {return _minMax.size();}
+	virtual void update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept);
+	QList<QGraphicsItem *> _items;
+	UPlot * _plot;
+
+private:
+	void removeItem(UPlotItem * item);
+
+private:
+	QString _name;
+	QPen _pen;
+	QBrush _brush;
+	qreal _xIncrement;
+	qreal _xStart;
+	bool _visible;
+	bool _valuesShown;
+	QVector<qreal> _minMax; // minX, maxX, minY, maxY
+	QGraphicsRectItem * _rootItem;
+	QColor _itemsColor;
+};
+
+
+/**
+ * A special UPlotCurve that shows as a line at the specified value, spanning all the UPlot.
+ */
+class RTABMAP_GUI_EXPORT UPlotCurveThreshold : public UPlotCurve
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor.
+	 */
+	UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
+	virtual ~UPlotCurveThreshold();
+	qreal getThreshold() const {return _threshold;}
+
+public Q_SLOTS:
+	/**
+	 * Set threshold value.
+	 */
+	void setThreshold(qreal threshold);
+	/**
+	 * Set orientation (Qt::Horizontal or Qt::Vertical).
+	 */
+	void setOrientation(Qt::Orientation orientation);
+
+protected:
+	friend class UPlot;
+	virtual void update(qreal scaleX, qreal scaleY, qreal offsetX, qreal offsetY, qreal xDir, qreal yDir, int maxItemsKept);
+	virtual bool isMinMaxValid() const {return false;}
+
+private:
+	Qt::Orientation _orientation;
+	qreal _threshold;
+};
+
+/**
+ * The UPlot axis object.
+ */
+class RTABMAP_GUI_EXPORT UPlotAxis : public QWidget
+{
+public:
+	/**
+	 * Constructor.
+	 */
+	UPlotAxis(Qt::Orientation orientation = Qt::Horizontal, qreal min=0, qreal max=1, QWidget * parent = 0);
+	virtual ~UPlotAxis();
+
+public:
+	/**
+	 * Set axis minimum and maximum values, compute the resulting
+	 * intervals depending on the size of the axis.
+	 */
+	void setAxis(qreal & min, qreal & max);
+	/**
+	 * Size of the border between the first line and the beginning of the widget.
+	 */
+	int border() const {return _border;}
+	/**
+	 * Interval step value.
+	 */
+	int step() const {return _step;}
+	/**
+	 * Number of intervals.
+	 */
+	int count() const {return _count;}
+	/**
+	 * Reverse the axis (for vertical :bottom->up, for horizontal :right->left)
+	 */
+	void setReversed(bool reversed); // Vertical :bottom->up, horizontal :right->left
+
+protected:
+	virtual void paintEvent(QPaintEvent * event);
+
+private:
+	Qt::Orientation _orientation;
+	qreal _min;
+	qreal _max;
+	int _count;
+	int _step;
+	bool _reversed;
+	int _gradMaxDigits;
+	int _border;
+};
+
+
+/**
+ * The UPlot legend item. Used internally by UPlot.
+ */
+class RTABMAP_GUI_EXPORT UPlotLegendItem : public QPushButton
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor.
+	 */
+	UPlotLegendItem(UPlotCurve * curve, QWidget * parent = 0);
+	virtual ~UPlotLegendItem();
+	const UPlotCurve * curve() const {return _curve;}
+	QPixmap createSymbol(const QPen & pen, const QBrush & brush);
+	void showStdDevMeanMax(bool shown);
+
+Q_SIGNALS:
+	void legendItemRemoved(const UPlotCurve *);
+	void moveUpRequest(UPlotLegendItem *);
+	void moveDownRequest(UPlotLegendItem *);
+
+private Q_SLOTS:
+	void updateStdDevMeanMax();
+
+protected:
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+
+private:
+	UPlotCurve * _curve;
+	QMenu * _menu;
+	QAction * _aChangeText;
+	QAction * _aResetText;
+	QAction * _aChangeColor;
+	QAction * _aCopyToClipboard;
+	QAction * _aShowStdDevMeanMax;
+	QAction * _aRemoveCurve;
+	QAction * _aMoveUp;
+	QAction * _aMoveDown;
+};
+
+/**
+ * The UPlot legend. Used internally by UPlot.
+ */
+class RTABMAP_GUI_EXPORT UPlotLegend : public QWidget
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor.
+	 */
+	UPlotLegend(QWidget * parent = 0);
+	virtual ~UPlotLegend();
+
+	void setFlat(bool on);
+	bool isFlat() const {return _flat;}
+	void addItem(UPlotCurve * curve);
+	bool remove(const UPlotCurve * curve);
+	QString getAllCurveDataAsText() const;
+
+private Q_SLOTS:
+	void removeLegendItem(const UPlotCurve * curve);
+	void moveUp(UPlotLegendItem * item);
+	void moveDown(UPlotLegendItem * item);
+
+Q_SIGNALS:
+	void legendItemRemoved(const UPlotCurve * curve);
+	void legendItemToggled(const UPlotCurve * curve, bool toggled);
+	void legendItemMoved(const UPlotCurve * curve, int);
+
+protected:
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+
+private Q_SLOTS:
+	void redirectToggled(bool);
+
+private:
+	bool _flat;
+	QMenu * _menu;
+	QAction * _aUseFlatButtons;
+	QAction * _aCopyAllCurvesToClipboard;
+	QAction * _aShowAllStdDevMeanMax;
+	QLayout * _contentLayout;
+	QScrollArea * _scrollArea;
+};
+
+
+/**
+ * Orientable QLabel. Inherit QLabel and let you to specify the orientation.
+ */
+class RTABMAP_GUI_EXPORT UOrientableLabel : public QLabel
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor.
+	 */
+	UOrientableLabel(const QString & text, Qt::Orientation orientation = Qt::Horizontal, QWidget * parent = 0);
+	virtual ~UOrientableLabel();
+	/**
+	 * Get orientation.
+	 */
+	Qt::Orientation orientation() const {return _orientation;}
+	/**
+	 * Set orientation (Qt::Vertical or Qt::Horizontal).
+	 */
+	void setOrientation(Qt::Orientation orientation);
+	QSize sizeHint() const;
+	QSize minimumSizeHint() const;
+protected:
+    virtual void paintEvent(QPaintEvent* event);
+private:
+    Qt::Orientation _orientation;
+};
+
+/**
+ * UPlot is a QWidget to create a plot like MATLAB, and
+ * incrementally add new values like a scope using Qt signals/slots.
+ * Many customizations can be done at runtime with the right-click menu.
+ * @image html UPlot.gif
+ * @image html UPlotMenu.png
+ *
+ * Example:
+ * @code
+ * #include "utilite/UPlot.h"
+ * #include <QApplication>
+ *
+ * int main(int argc, char * argv[])
+ * {
+ *	QApplication app(argc, argv);
+ *	UPlot plot;
+ *	UPlotCurve * curve = plot.addCurve("My curve");
+ *	qreal y[10] = {0, 1, 2, 3, -3, -2, -1, 0, 1, 2};
+ *	curve->addValues(std::vector<qreal>(y, y+10));
+ *	plot.showGrid(true);
+ *	plot.setGraphicsView(true);
+ *	plot.show();
+ *	app.exec();
+ * 	return 0;
+ * }
+ * @endcode
+ * @image html SimplePlot.tiff
+ *
+ *
+ */
+class RTABMAP_GUI_EXPORT UPlot : public QWidget
+{
+	Q_OBJECT
+
+public:
+	/**
+	 * Constructor.
+	 */
+	UPlot(QWidget * parent = 0);
+	virtual ~UPlot();
+
+	/**
+	 * Add a curve. The returned curve doesn't need to be deallocated (UPlot keeps the ownership).
+	 */
+	UPlotCurve * addCurve(const QString & curveName, const QColor & color = QColor());
+	/**
+	 * Add a curve. Ownership is transferred to UPlot if ownershipTransferred=true.
+	 */
+	bool addCurve(UPlotCurve * curve, bool ownershipTransferred = true);
+	/**
+	 * Get all curve names.
+	 */
+	QStringList curveNames() const;
+	bool contains(const QString & curveName) const;
+	bool isThreshold(const QString & curveName) const;
+	double getThresholdValue(const QString & curveName) const;
+	void removeCurves();
+	QString getAllCurveDataAsText() const;
+	/**
+	 * Add a threshold to the plot.
+	 */
+	UPlotCurveThreshold * addThreshold(const QString & name, qreal value, Qt::Orientation orientation = Qt::Horizontal);
+	QString title() const {return this->objectName();}
+	QPen getRandomPenColored();
+	void showLegend(bool shown);
+	void showGrid(bool shown);
+	void showRefreshRate(bool shown);
+	void trackMouse(bool tracking);
+	void keepAllData(bool kept);
+	void showXAxis(bool shown) {_horizontalAxis->setVisible(shown);}
+	void showYAxis(bool shown) {_verticalAxis->setVisible(shown);}
+	void setVariableXAxis() {_fixedAxis[0] = false;}
+	void setVariableYAxis() {_fixedAxis[1] = false;}
+	void setFixedXAxis(qreal x1, qreal x2);
+	void setFixedYAxis(qreal y1, qreal y2);
+	void setMaxVisibleItems(int maxVisibleItems);
+	void setTitle(const QString & text);
+	void setXLabel(const QString & text);
+	void setYLabel(const QString & text, Qt::Orientation orientation = Qt::Vertical);
+	void setWorkingDirectory(const QString & workingDirectory);
+	void setGraphicsView(bool on);
+	void setBackgroundColor(const QColor & color);
+	QRectF sceneRect() const;
+
+public Q_SLOTS:
+	/**
+	 *
+	 * Remove a curve. If UPlot is the parent of the curve, the curve is deleted.
+	 */
+	void removeCurve(const UPlotCurve * curve);
+	void showCurve(const UPlotCurve * curve, bool shown);
+	void updateAxis(); //reset axis and recompute it with all curves minMax
+	/**
+	 *
+	 * Clear all curves' data.
+	 */
+	void clearData();
+
+	void frameData(bool xAxis = true, bool yAxis = false);
+
+private Q_SLOTS:
+	void captureScreen();
+	void updateAxis(const UPlotCurve * curve);
+	void moveCurve(const UPlotCurve *, int index);
+
+protected:
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+	virtual void paintEvent(QPaintEvent * event);
+	virtual void resizeEvent(QResizeEvent * event);
+	virtual void mousePressEvent(QMouseEvent * event);
+	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void mouseReleaseEvent(QMouseEvent * event);
+	virtual void mouseDoubleClickEvent(QMouseEvent * event);
+
+private:
+	friend class UPlotCurve;
+	void addItem(QGraphicsItem * item);
+
+private:
+	void replot(QPainter * painter);
+	bool updateAxis(qreal x, qreal y);
+	bool updateAxis(qreal x1, qreal x2, qreal y1, qreal y2);
+	void setupUi();
+	void createActions();
+	void createMenus();
+	void selectScreenCaptureFormat();
+	bool mousePosToValue(const QPoint & pos, qreal & x, qreal & y);
+
+private:
+	UPlotLegend * _legend;
+	QGraphicsView * _view;
+	QGraphicsItem * _sceneRoot;
+	QWidget * _graphicsViewHolder;
+	qreal _axisMaximums[4]; // {x1->x2, y1->y2}
+	bool _axisMaximumsSet[4]; // {x1->x2, y1->y2}
+	bool _fixedAxis[2];
+	UPlotAxis * _verticalAxis;
+	UPlotAxis * _horizontalAxis;
+	int _penStyleCount;
+	int _maxVisibleItems;
+	QList<QGraphicsLineItem *> hGridLines;
+	QList<QGraphicsLineItem *> vGridLines;
+	QList<UPlotCurve*> _curves;
+	QLabel * _title;
+	QLabel * _xLabel;
+	UOrientableLabel * _yLabel;
+	QLabel * _refreshRate;
+	QString _workingDirectory;
+	QElapsedTimer _refreshIntervalTime;
+	int _lowestRefreshRate;
+	QElapsedTimer _refreshStartTime;
+	QString _autoScreenCaptureFormat;
+	QPoint _mousePressedPos;
+	QPoint _mouseCurrentPos;
+	QColor _bgColor;
+
+	QMenu * _menu;
+	QAction * _aShowLegend;
+	QAction * _aShowGrid;
+	QAction * _aKeepAllData;
+	QAction * _aLimit0;
+	QAction * _aLimit10;
+	QAction * _aLimit50;
+	QAction * _aLimit100;
+	QAction * _aLimit500;
+	QAction * _aLimit1000;
+	QAction * _aLimitCustom;
+	QAction * _aAddVerticalLine;
+	QAction * _aAddHorizontalLine;
+	QAction * _aChangeTitle;
+	QAction * _aChangeXLabel;
+	QAction * _aChangeYLabel;
+	QAction * _aChangeBackgroundColor;
+	QAction * _aYLabelVertical;
+	QAction * _aShowRefreshRate;
+	QAction * _aMouseTracking;
+	QAction * _aSaveFigure;
+	QAction * _aAutoScreenCapture;
+	QAction * _aClearData;
+	QAction * _aGraphicsView;
+};
+
+#endif /* UPLOT_H_ */

rtabmap/guilib/src/3rdParty/QMultiComboBox.cpp

@@ -0,0 +1,237 @@
+/****************************************************************************
+**
+** Copyright (c) 2012 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** QMultiComboBox is free to use unter the terms of the LGPL 2.1 License in
+** Free and Commercial Products.
+****************************************************************************/
+
+#include "QMultiComboBox.h"
+#include <QApplication>
+#include <QCoreApplication>
+#include <QWindow>
+#include <QScreen>
+
+#include "rtabmap/utilite/ULogger.h"
+
+QMultiComboBox::QMultiComboBox(QWidget *widget ) :
+    QComboBox(widget),
+    popheight_(0),
+    screenbound_(50),
+    popframe_(NULL, Qt::Popup)
+{
+
+    SetDisplayText("Not Set");
+
+    // setup the popup list
+    vlist_.setSelectionMode(QAbstractItemView::MultiSelection);
+    vlist_.setSelectionBehavior(QAbstractItemView::SelectItems);
+    vlist_.clearSelection();
+    popframe_.setLayout(new QVBoxLayout());
+    popframe_.layout()->addWidget(&vlist_);
+    popframe_.layout()->setContentsMargins(0,0,0,0);
+
+    connect(&vlist_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(scanItemSelect(QListWidgetItem*)));
+
+}
+
+
+QMultiComboBox::~QMultiComboBox()
+{
+    disconnect(&vlist_,0,0,0);
+}
+
+
+void QMultiComboBox::SetDisplayText(QString text)
+{
+    m_DisplayText_ = text;
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
+    const int textWidth = fontMetrics().horizontalAdvance(text);
+#else
+    const int textWidth = fontMetrics().width(text);
+#endif
+    setMinimumWidth(textWidth + 30);
+    updateGeometry();
+    repaint();
+}
+
+
+QString QMultiComboBox::GetDisplayText() const
+{
+    return m_DisplayText_;
+}
+
+
+void QMultiComboBox::setPopupHeight(int h)
+{
+    popheight_ = h;
+}
+
+
+void QMultiComboBox::paintEvent(QPaintEvent *e)
+{
+    QStylePainter painter(this);
+    painter.setPen(palette().color(QPalette::Text));
+    // draw the combobox frame, focusrect and selected etc.
+    QStyleOptionComboBox opt;
+
+    initStyleOption(&opt);
+    opt.currentText = m_DisplayText_;
+    painter.drawComplexControl(QStyle::CC_ComboBox, opt);
+    // draw the icon and text
+    painter.drawControl(QStyle::CE_ComboBoxLabel, opt);
+}
+
+
+void QMultiComboBox::showPopup()
+{
+    QRect rec = QRect(geometry());
+
+    //QPoint p = this->mapToGlobal(QPoint(0,rec.height()));
+    //QRect rec2(p , p + QPoint(rec.width(), rec.height()));
+
+    // get the two possible list points and height
+    QRect screen = this->window()->windowHandle()->screen()->availableGeometry();
+    QPoint above = this->mapToGlobal(QPoint(0,0));
+    int aboveHeight = above.y() - screen.y();
+    QPoint below = this->mapToGlobal(QPoint(0,rec.height()));
+    int belowHeight = screen.bottom() - below.y();
+
+	// compute width
+	int textWidth = vlist_.sizeHint().width();
+
+    // first activate it with height 1px to get all the items initialized
+    QRect rec2;
+    rec2.setTopLeft(below);
+    rec2.setWidth(textWidth>rec.width()? textWidth:rec.width());
+    rec2.setHeight(rec.height());
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+    QCoreApplication::processEvents();
+
+    // determine rect
+    int contheight = vlist_.count()*vlist_.sizeHintForRow(0) + 4; // +4 - should be determined by margins?
+    belowHeight = std::min(abs(belowHeight)-screenbound_, contheight);
+    aboveHeight = std::min(abs(aboveHeight)-screenbound_, contheight);
+    if (popheight_ > 0) // fixed
+    {
+        rec2.setHeight(popheight_);
+    }
+    else // dynamic
+    {
+        // do we use below or above
+        if (belowHeight==contheight || belowHeight>aboveHeight)
+        {
+            rec2.setTopLeft(below);
+            rec2.setHeight(belowHeight);
+        }
+        else
+        {
+            rec2.setTopLeft(above - QPoint(0,aboveHeight));
+            rec2.setHeight(aboveHeight);
+        }
+    }
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+}
+
+
+void QMultiComboBox::hidePopup()
+{
+    popframe_.hide();
+}
+
+
+void QMultiComboBox::addItem ( const QString & text, const QVariant & userData)
+{
+    QListWidgetItem* wi = new QListWidgetItem(text);
+    wi->setFlags(wi->flags() | Qt::ItemIsUserCheckable);
+    if (userData.toBool())
+        wi->setCheckState(Qt::Checked);
+    else
+        wi->setCheckState(Qt::Unchecked);
+    vlist_.addItem(wi);
+	vlist_.setMinimumWidth(vlist_.sizeHintForColumn(0));
+}
+
+
+int QMultiComboBox::count()
+{
+    return vlist_.count();
+}
+
+
+void QMultiComboBox::setCurrentIndex(int index)
+{
+  //  cout << __FUNCTION__ << "DONT USE THIS ................" << endl;
+}
+
+
+QString QMultiComboBox::currentText()
+{
+    return vlist_.currentItem()->text();
+}
+
+
+QString QMultiComboBox::itemText(int row)
+{
+    return vlist_.item(row)->text();
+}
+
+
+QVariant QMultiComboBox::itemData(int row)
+{
+    QListWidgetItem* item = vlist_.item(row);
+    if (item->checkState() == Qt::Checked) return QVariant(true);
+    return QVariant(false);
+}
+
+void QMultiComboBox::setItemChecked(int row, bool checked)
+{
+	QListWidgetItem* item = vlist_.item(row);
+	bool wasChecked = item->checkState() == Qt::Checked;
+	if (wasChecked != checked)
+	{
+		item->setCheckState(checked?Qt::Checked:Qt::Unchecked);
+		Q_EMIT itemChanged();
+	}
+}
+
+
+void QMultiComboBox::scanItemSelect(QListWidgetItem* item)
+{
+
+    QList<QListWidgetItem*> list = vlist_.selectedItems();
+    for (int i = 0; i < list.count(); i++)
+    {
+        if (item->checkState() == Qt::Checked)
+        {
+            list[i]->setCheckState(Qt::Checked);
+        }
+        else
+        {
+            list[i]->setCheckState(Qt::Unchecked);
+        }
+        list[i]->setSelected(false);
+    }
+    Q_EMIT itemChanged();
+}
+
+void QMultiComboBox::initStyleOption(QStyleOptionComboBox *option) const
+{
+    //Initializes the state, direction, rect, palette, and fontMetrics member variables based on the specified widget.
+    //This is a convenience function; the member variables can also be initialized manually.
+    option->initFrom(this);
+
+}
+
+void QMultiComboBox::clear()
+{
+	vlist_.clear();
+	QComboBox::clear();
+}
+

rtabmap/guilib/src/3rdParty/QMultiComboBox.h

@@ -0,0 +1,91 @@
+/****************************************************************************
+**
+** Copyright (C) 2010 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** Observe the License Information
+**
+****************************************************************************/
+
+#ifndef __MULTIBOXCOMBO_H__
+#define __MULTIBOXCOMBO_H__
+
+#include <iostream>
+
+#include <QComboBox>
+#include <QListWidget>
+#include <QVBoxLayout>
+#include <QStylePainter>
+
+class QMultiComboBox: public QComboBox
+{
+    Q_OBJECT
+public:
+
+    /// Constructor
+    QMultiComboBox(QWidget *widget = 0);
+
+    virtual ~QMultiComboBox();
+
+    /// the main display text
+    void SetDisplayText(QString text);
+
+    /// get the main display text
+    QString GetDisplayText() const;
+
+    /// add a item to the list
+    void addItem(const QString& text, const QVariant& userData = QVariant());
+
+    /// custom paint
+    virtual void paintEvent(QPaintEvent *e);
+
+    /// set the height of the popup
+    void setPopupHeight(int h);
+
+    /// replace standard QComboBox Popup
+    void showPopup();
+    void hidePopup();
+
+    /// replace neccessary data access
+    int count();
+    void setCurrentIndex(int index);
+    QString currentText();
+    QString itemText(int row);
+    QVariant itemData(int row);
+    void setItemChecked(int row, bool checked);
+
+Q_SIGNALS:
+    /// item changed
+    void itemChanged();
+
+public Q_SLOTS:
+
+    /// react on changes of the item checkbox
+    void scanItemSelect(QListWidgetItem* item);
+
+    /// the init style
+    void initStyleOption(QStyleOptionComboBox *option) const;
+
+    void clear();
+
+protected:
+
+    /// the height of the popup
+    int popheight_;
+
+    /// lower/upper screen bound
+    int screenbound_;
+
+    /// hold the main display text
+    QString m_DisplayText_;
+
+    /// popup frame
+    QFrame popframe_;
+
+    /// multi selection list in the popup frame
+    QListWidget vlist_;
+
+};
+
+#endif

rtabmap/guilib/src/3rdParty/lgpl-2.1.txt

@@ -0,0 +1,504 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; 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 2.1 of the License, or (at your option) any later version.
+
+    This library 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 library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
+

rtabmap/guilib/src/AboutDialog.cpp

@@ -0,0 +1,307 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/AboutDialog.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/Optimizer.h"
+#include "ui_aboutDialog.h"
+#include <opencv2/core/version.hpp>
+#include <pcl/pcl_config.h>
+#include <vtkVersion.h>
+
+namespace rtabmap {
+
+AboutDialog::AboutDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_aboutDialog();
+	_ui->setupUi(this);
+	QString version = Parameters::getVersion().c_str();
+	QString cv_version = CV_VERSION;
+#if CV_MAJOR_VERSION < 3
+  #ifdef RTABMAP_NONFREE
+	_ui->label_opencv_license->setText("Not Commercial [With nonfree module]");
+  #else
+	_ui->label_opencv_license->setText("BSD [Without nonfree module]");
+  #endif
+#elif defined(HAVE_OPENCV_XFEATURES2D)
+  #ifdef RTABMAP_NONFREE
+	_ui->label_opencv_license->setText("Not Commercial [With xfeatures2d and nonfree modules]");
+  #else
+    #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION<5)
+	  _ui->label_opencv_license->setText("BSD [With xfeatures2d module]");
+    #else
+	  _ui->label_opencv_license->setText("Apache 3 [With xfeatures2d module]");
+    #endif
+  #endif
+#else
+  #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION<5)
+	_ui->label_opencv_license->setText("BSD [Without xfeatures2d and nonfree modules]");
+  #else
+	_ui->label_opencv_license->setText("Apache 3 [Without xfeatures2d and nonfree modules]");
+  #endif
+#endif
+	_ui->label_version->setText(version);
+	_ui->label_opencv_version->setText(cv_version);
+	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
+	_ui->label_vtk_version->setText(vtkVersion::GetVTKVersion());
+	_ui->label_qt_version->setText(qVersion());
+#ifdef RTABMAP_ORB_OCTREE
+	_ui->label_orboctree->setText("Yes");
+	_ui->label_orboctree_license->setEnabled(true);
+#else
+	_ui->label_orboctree->setText("No");
+	_ui->label_orboctree_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_TORCH
+	_ui->label_sptorch->setText("Yes");
+	_ui->label_sptorch_license->setEnabled(true);
+#else
+	_ui->label_sptorch->setText("No");
+	_ui->label_sptorch_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_PYTHON
+	_ui->label_pymatcher->setText("Yes");
+	_ui->label_pymatcher_license->setEnabled(true);
+#else
+	_ui->label_pymatcher->setText("No");
+	_ui->label_pymatcher_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_FASTCV
+	_ui->label_fastcv->setText("Yes");
+	_ui->label_fastcv_license->setEnabled(true);
+#else
+	_ui->label_fastcv->setText("No");
+	_ui->label_fastcv_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_PDAL
+	_ui->label_pdal->setText("Yes");
+	_ui->label_pdal_license->setEnabled(true);
+#else
+	_ui->label_pdal->setText("No");
+	_ui->label_pdal_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_LIBLAS
+	_ui->label_liblas->setText("Yes");
+	_ui->label_liblas_license->setEnabled(true);
+#else
+	_ui->label_liblas->setText("No");
+	_ui->label_liblas_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_CUDASIFT
+	_ui->label_cudasift->setText("Yes");
+	_ui->label_cudasift_license->setEnabled(true);
+#else
+	_ui->label_cudasift->setText("No");
+	_ui->label_cudasift_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_OCTOMAP
+	_ui->label_octomap->setText("Yes");
+	_ui->label_octomap_license->setEnabled(true);
+#else
+	_ui->label_octomap->setText("No");
+	_ui->label_octomap_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_GRIDMAP
+	_ui->label_gridmap->setText("Yes");
+	_ui->label_gridmap_license->setEnabled(true);
+#else
+	_ui->label_gridmap->setText("No");
+	_ui->label_gridmap_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_CPUTSDF
+	_ui->label_cputsdf->setText("Yes");
+	_ui->label_cputsdf_license->setEnabled(true);
+#else
+	_ui->label_cputsdf->setText("No");
+	_ui->label_cputsdf_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_OPENCHISEL
+	_ui->label_openchisel->setText("Yes");
+#else
+	_ui->label_openchisel->setText("No");
+#endif
+#ifdef RTABMAP_ALICE_VISION
+	_ui->label_aliceVision->setText("Yes");
+	_ui->label_aliceVision_license->setEnabled(true);
+#else
+	_ui->label_aliceVision->setText("No");
+	_ui->label_aliceVision_license->setEnabled(false);
+#endif
+
+	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
+	_ui->label_freenect_license->setEnabled(CameraFreenect::available());
+	_ui->label_openni->setText(CameraOpenni::available()?"Yes":"No");
+	_ui->label_openni_license->setEnabled(CameraOpenni::available());
+	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
+	_ui->label_openni2_license->setEnabled(CameraOpenNI2::available());
+	_ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No");
+	_ui->label_freenect2_license->setEnabled(CameraFreenect2::available());
+	_ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No");
+	_ui->label_realsense_license->setEnabled(CameraRealSense::available());
+	_ui->label_realsense2->setText(CameraRealSense2::available() ? "Yes" : "No");
+	_ui->label_realsense2_license->setEnabled(CameraRealSense2::available());
+	_ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No");
+	_ui->label_dc1394_license->setEnabled(CameraStereoDC1394::available());
+	_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
+	_ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No");
+	_ui->label_zedOC->setText(CameraStereoZedOC::available()?"Yes":"No");
+	_ui->label_zedOC_license->setEnabled(CameraStereoZedOC::available());
+	_ui->label_k4w2->setText(CameraK4W2::available() ? "Yes" : "No");
+	_ui->label_k4a->setText(CameraK4A::available() ? "Yes" : "No");
+	_ui->label_mynteye->setText(CameraMyntEye::available() ? "Yes" : "No");
+	_ui->label_depthai->setText(CameraDepthAI::available() ? "Yes" : "No");
+	_ui->label_depthai_license->setEnabled(CameraDepthAI::available());
+	_ui->label_xvsdk->setText(CameraSeerSense::available() ? "Yes" : "No");
+
+	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
+	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);
+	_ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No");
+	_ui->label_g2o_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O)?true:false);
+	_ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No");
+	_ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false);
+	_ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No");
+	_ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false);
+	_ui->label_ceres->setText(Optimizer::isAvailable(Optimizer::kTypeCeres)?"Yes":"No");
+	_ui->label_ceres_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCeres)?true:false);
+
+#ifdef RTABMAP_MRPT
+	_ui->label_mrpt->setText("Yes");
+	_ui->label_mrpt_license->setEnabled(true);
+#else
+	_ui->label_mrpt->setText("No");
+	_ui->label_mrpt_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_POINTMATCHER
+	_ui->label_libpointmatcher->setText("Yes");
+	_ui->label_libpointmatcher_license->setEnabled(true);
+#else
+	_ui->label_libpointmatcher->setText("No");
+	_ui->label_libpointmatcher_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_CCCORELIB
+	_ui->label_cccorelib->setText("Yes");
+	_ui->label_cccorelib_license->setEnabled(true);
+#else
+	_ui->label_cccorelib->setText("No");
+	_ui->label_cccorelib_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OPEN3D
+	_ui->label_open3d->setText("Yes");
+	_ui->label_open3d_license->setEnabled(true);
+#else
+	_ui->label_open3d->setText("No");
+	_ui->label_open3d_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_FOVIS
+	_ui->label_fovis->setText("Yes");
+	_ui->label_fovis_license->setEnabled(true);
+#else
+	_ui->label_fovis->setText("No");
+	_ui->label_fovis_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_VISO2
+	_ui->label_viso2->setText("Yes");
+	_ui->label_viso2_license->setEnabled(true);
+#else
+	_ui->label_viso2->setText("No");
+	_ui->label_viso2_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_DVO
+	_ui->label_dvo->setText("Yes");
+	_ui->label_dvo_license->setEnabled(true);
+#else
+	_ui->label_dvo->setText("No");
+	_ui->label_dvo_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_ORB_SLAM
+#if RTABMAP_ORB_SLAM == 3
+	_ui->label_orbslam_title->setText("With ORB SLAM3 :");
+#elif RTABMAP_ORB_SLAM == 2
+	_ui->label_orbslam_title->setText("With ORB SLAM2 :");
+#endif
+	_ui->label_orbslam->setText("Yes");
+	_ui->label_orbslam_license->setEnabled(true);
+#else
+	_ui->label_orbslam->setText("No");
+	_ui->label_orbslam_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OKVIS
+	_ui->label_okvis->setText("Yes");
+	_ui->label_okvis_license->setEnabled(true);
+#else
+	_ui->label_okvis->setText("No");
+	_ui->label_okvis_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_LOAM
+	_ui->label_loam->setText("Yes");
+	_ui->label_loam_license->setEnabled(true);
+#else
+	_ui->label_loam->setText("No");
+	_ui->label_loam_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_MSCKF_VIO
+	_ui->label_msckf->setText("Yes");
+	_ui->label_msckf_license->setEnabled(true);
+#else
+	_ui->label_msckf->setText("No");
+	_ui->label_msckf_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_VINS
+	_ui->label_vins_fusion->setText("Yes");
+	_ui->label_vins_fusion_license->setEnabled(true);
+#else
+	_ui->label_vins_fusion->setText("No");
+	_ui->label_vins_fusion_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OPENVINS
+	_ui->label_openvins->setText("Yes");
+	_ui->label_openvins_license->setEnabled(true);
+#else
+	_ui->label_openvins->setText("No");
+	_ui->label_openvins_license->setEnabled(false);
+#endif
+
+}
+
+AboutDialog::~AboutDialog()
+{
+	delete _ui;
+}
+
+}

rtabmap/guilib/src/CMakeLists.txt

@@ -0,0 +1,261 @@
+
+
+### Qt Gui stuff ###
+SET(headers_ui 
+	../include/${PROJECT_PREFIX}/gui/MainWindow.h
+	../include/${PROJECT_PREFIX}/gui/PreferencesDialog.h
+	../include/${PROJECT_PREFIX}/gui/DatabaseViewer.h
+	../include/${PROJECT_PREFIX}/gui/AboutDialog.h
+	../include/${PROJECT_PREFIX}/gui/ConsoleWidget.h
+	../include/${PROJECT_PREFIX}/gui/ImageView.h
+	../include/${PROJECT_PREFIX}/gui/PdfPlot.h
+	../include/${PROJECT_PREFIX}/gui/StatsToolBox.h
+	../include/${PROJECT_PREFIX}/gui/ProgressDialog.h
+	../include/${PROJECT_PREFIX}/utilite/UPlot.h
+	../include/${PROJECT_PREFIX}/utilite/UImageView.h
+	../include/${PROJECT_PREFIX}/gui/CloudViewer.h
+	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
+	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
+	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
+	../include/${PROJECT_PREFIX}/gui/CameraViewer.h
+	../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportDialog.h
+	../include/${PROJECT_PREFIX}/gui/PostProcessingDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportCloudsDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportBundlerDialog.h
+	../include/${PROJECT_PREFIX}/gui/MapVisibilityWidget.h
+	../include/${PROJECT_PREFIX}/gui/GraphViewer.h
+	../include/${PROJECT_PREFIX}/gui/CreateSimpleCalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/ParametersToolBox.h
+	../include/${PROJECT_PREFIX}/gui/DepthCalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/EditConstraintDialog.h
+	./3rdParty/QMultiComboBox.h
+	../include/${PROJECT_PREFIX}/gui/TexturingState.h
+	../include/${PROJECT_PREFIX}/gui/RecoveryState.h
+	../include/${PROJECT_PREFIX}/gui/EditDepthArea.h
+	../include/${PROJECT_PREFIX}/gui/EditMapArea.h
+	../include/${PROJECT_PREFIX}/gui/MultiSessionLocWidget.h
+	../include/${PROJECT_PREFIX}/gui/MultiSessionLocSubView.h
+	../include/${PROJECT_PREFIX}/gui/LinkRefiningDialog.h
+)
+
+SET(qrc 
+    ./GuiLib.qrc
+)
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+    set(CMAKE_AUTOUIC_SEARCH_PATHS ./ui/)
+ENDIF()
+
+
+SET(SRC_FILES 
+	./MainWindow.cpp
+	./PreferencesDialog.cpp
+	./KeypointItem.cpp
+	./ImageView.cpp
+	./PdfPlot.cpp
+	./StatsToolBox.cpp
+	./ProgressDialog.cpp
+	./AboutDialog.cpp
+	./ConsoleWidget.cpp
+	./DatabaseViewer.cpp
+	./utilite/UPlot.cpp
+	./CloudViewer.cpp
+	./CloudViewerCellPicker.cpp
+	./CloudViewerInteractorStyle.cpp
+	./OdometryViewer.cpp
+	./LoopClosureViewer.cpp
+	./DataRecorder.cpp
+	./CameraViewer.cpp
+	./CalibrationDialog.cpp
+	./ExportDialog.cpp
+	./ExportBundlerDialog.cpp
+	./PostProcessingDialog.cpp
+	./ExportCloudsDialog.cpp
+	./EditConstraintDialog.cpp
+	./MapVisibilityWidget.cpp
+	./GraphViewer.cpp
+	./EditDepthArea.cpp
+	./EditMapArea.cpp
+	./MultiSessionLocWidget.cpp
+	./MultiSessionLocSubView.cpp
+	./CreateSimpleCalibrationDialog.cpp
+	./ParametersToolBox.cpp
+	./DepthCalibrationDialog.cpp
+	./LinkRefiningDialog.cpp
+	./3rdParty/QMultiComboBox.cpp
+	./opencv/vtkImageMatSource.cpp
+	${qrc}
+	${headers_ui}
+)
+
+# to get includes in visual studio
+IF(MSVC)
+    FILE(GLOB HEADERS
+        ../include/${PROJECT_PREFIX}/gui/*.h
+    )
+    SET(SRC_FILES ${SRC_FILES} ${HEADERS})
+ENDIF(MSVC)
+
+SET(INCLUDE_DIRS
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
+)
+
+IF(QT4_FOUND)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(QT4_FOUND)
+
+SET(LIBRARIES "")
+SET(PUBLIC_LIBRARIES "")
+SET(PUBLIC_INCLUDE_DIRS "")
+
+IF(Qt6_FOUND)
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt6::Widgets Qt6::Core Qt6::Gui Qt6::OpenGL)
+    SET(LIBRARIES ${LIBRARIES} Qt6::PrintSupport)
+    IF(Qt6Svg_FOUND)
+        SET(LIBRARIES ${LIBRARIES} Qt6::Svg)
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${PUBLIC_INCLUDE_DIRS}
+        ${Qt6Widgets_INCLUDE_DIRS}
+        ${Qt6Core_INCLUDE_DIRS}
+        ${Qt6Gui_INCLUDE_DIRS}
+        ${Qt6OpenGL_INCLUDE_DIRS})
+ELSEIF(Qt5_FOUND)
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt5::Widgets Qt5::Core Qt5::Gui Qt5::OpenGL)
+    SET(LIBRARIES ${LIBRARIES} Qt5::PrintSupport)
+    IF(Qt5Svg_FOUND)
+        SET(LIBRARIES ${LIBRARIES} Qt5::Svg)
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${PUBLIC_INCLUDE_DIRS}
+        ${Qt5Widgets_INCLUDE_DIRS}
+        ${Qt5Core_INCLUDE_DIRS}
+        ${Qt5Gui_INCLUDE_DIRS}
+        ${Qt5OpenGL_INCLUDE_DIRS})
+ELSE()
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} ${QTCORE_LIBRARY} ${QTGUI_LIBRARY})
+    IF(QTSVG_FOUND)
+        SET(LIBRARIES ${LIBRARIES} ${QTSVG_LIBRARY})
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${QTCORE_INCLUDE_DIRS}
+        ${QTGUI_INCLUDE_DIRS})
+ENDIF()
+
+IF(CPUTSDF_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${CPUTSDF_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${CPUTSDF_LIBRARIES}
+	)
+ENDIF(CPUTSDF_FOUND)
+
+IF(open_chisel_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${open_chisel_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${open_chisel_LIBRARIES}
+	)
+ENDIF(open_chisel_FOUND)
+
+IF(VTK_USE_QVTK)
+	SET(INCLUDE_DIRS ${INCLUDE_DIRS} ${QVTK_INCLUDE_DIR})
+	SET(LIBRARIES ${LIBRARIES} ${QVTK_LIBRARY})
+ENDIF(VTK_USE_QVTK)
+
+#include files
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+add_definitions(${PCL_DEFINITIONS})
+
+# Include presets
+SET(RESOURCES
+	${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini
+	${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini
+)
+
+foreach(arg ${RESOURCES})
+   get_filename_component(filename ${arg} NAME)
+   string(REPLACE "." "_" output ${filename})
+   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
+   set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON)
+endforeach(arg ${RESOURCES})
+
+ADD_CUSTOM_COMMAND(
+	OUTPUT ${RESOURCES_HEADERS}
+	COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+	COMMENT "[Creating resources]"
+	DEPENDS ${RESOURCES}
+)
+
+# create a library from the source files
+ADD_LIBRARY(rtabmap_gui ${SRC_FILES} ${RESOURCES_HEADERS})
+ADD_LIBRARY(rtabmap::gui ALIAS rtabmap_gui)
+
+generate_export_header(rtabmap_gui
+ DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)
+
+target_include_directories(rtabmap_gui PUBLIC 
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS}>"
+  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
+
+TARGET_LINK_LIBRARIES(rtabmap_gui 
+  PUBLIC
+    rtabmap_core ${PUBLIC_LIBRARIES}
+  PRIVATE
+    ${LIBRARIES}
+)
+
+SET_TARGET_PROPERTIES(
+     rtabmap_gui
+   PROPERTIES
+     VERSION ${RTABMAP_VERSION}
+     SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}
+     EXPORT_NAME "gui"
+	 AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+INSTALL(TARGETS rtabmap_gui EXPORT rtabmap_guiTargets
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel
+        ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel)
+
+configure_file(
+  ${CMAKE_CURRENT_BINARY_DIR}/rtabmap_gui_export.h
+  ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/gui/rtabmap_gui_export.h
+  COPYONLY)
+  
+install(
+  DIRECTORY
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX}
+    ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}
+  DESTINATION
+    ${INSTALL_INCLUDE_DIR}
+  COMPONENT
+    devel
+  FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+)
+export(EXPORT rtabmap_guiTargets
+  FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_guiTargets.cmake"
+  NAMESPACE rtabmap::
+)
+install(EXPORT rtabmap_guiTargets
+  FILE
+    ${PROJECT_NAME}_guiTargets.cmake
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  NAMESPACE rtabmap::
+  COMPONENT
+    devel
+)

rtabmap/guilib/src/CameraViewer.cpp

@@ -0,0 +1,324 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/SensorEvent.h>
+#include "rtabmap/gui/CameraViewer.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/MarkerDetector.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <QtCore/QMetaType>
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QLabel>
+#include <QSpinBox>
+#include <QDialogButtonBox>
+#include <QCheckBox>
+#include <QPushButton>
+
+namespace rtabmap {
+
+
+CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
+		QDialog(parent),
+	imageView_(new ImageView(this)),
+	cloudView_(new CloudViewer(this)),
+	processingImages_(false),
+	parameters_(parameters),
+	markerDetector_(0)
+{
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+	imageView_->setVisible(false);
+	QHBoxLayout * layout = new QHBoxLayout();
+	layout->setContentsMargins(0,0,0,0);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QLabel * decimationLabel = new QLabel("Decimation", this);
+	decimationSpin_ = new QSpinBox(this);
+	decimationSpin_->setMinimum(-16);
+	decimationSpin_->setMaximum(16);
+	decimationSpin_->setValue(2);
+
+	pause_ = new QPushButton("Pause", this);
+	pause_->setCheckable(true);
+	showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
+	showCloudCheckbox_->setEnabled(false);
+	showCloudCheckbox_->setChecked(true);
+	showScanCheckbox_ = new QCheckBox("Show scan", this);
+	showScanCheckbox_->setEnabled(false);
+	showScanCheckbox_->setChecked(true);
+
+	markerCheckbox_ = new QCheckBox("Detect markers", this);
+#ifdef HAVE_OPENCV_ARUCO
+	markerCheckbox_->setEnabled(true);
+	markerDetector_ = new MarkerDetector(parameters);
+#else
+	markerCheckbox_->setEnabled(false);
+	markerCheckbox_->setToolTip("Disabled: RTAB-Map is not built with OpenCV's aruco module.");
+#endif
+	markerCheckbox_->setChecked(false);
+
+	imageSizeLabel_ = new QLabel(this);
+
+	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
+	buttonBox->setStandardButtons(QDialogButtonBox::Close);
+	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	QHBoxLayout * layout2 = new QHBoxLayout();
+	layout2->addWidget(pause_);
+	layout2->addWidget(decimationLabel);
+	layout2->addWidget(decimationSpin_);
+	layout2->addWidget(showCloudCheckbox_);
+	layout2->addWidget(showScanCheckbox_);
+	layout2->addWidget(markerCheckbox_);
+	layout2->addWidget(imageSizeLabel_);
+	layout2->addStretch(1);
+	layout2->addWidget(buttonBox);
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setContentsMargins(0,0,0,0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addLayout(layout2);
+
+	this->setLayout(vlayout);
+}
+
+CameraViewer::~CameraViewer()
+{
+	this->unregisterFromEventsManager();
+	delete markerDetector_;
+}
+
+void CameraViewer::setDecimation(int value)
+{
+	decimationSpin_->setValue(value);
+}
+
+void CameraViewer::showImage(const rtabmap::SensorData & data)
+{
+	processingImages_ = true;
+	QString sizes;
+
+	cv::Mat left;
+	cv::Mat depthOrRight;
+	LaserScan scan;
+	if( !data.imageRaw().empty() || !data.imageCompressed().empty() ||
+		!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() ||
+		!data.laserScanRaw().empty() || !data.laserScanCompressed().empty())
+	{
+		data.uncompressDataConst(
+			!data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0, 
+			!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0,
+			!data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0);
+	}
+
+	imageView_->setVisible(!left.empty() || !left.empty());
+	std::map<int, MarkerInfo> detections;
+	if(!left.empty())
+	{
+		std::vector<CameraModel> models;
+		if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+		{
+			models = data.cameraModels();
+			if(models.empty())
+			{
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				{
+					models.push_back(data.stereoCameraModels()[i].left());
+				}
+			}
+		}
+			
+		if(!models.empty() && models[0].isValidForProjection())
+		{
+			cv::Mat imageWithDetections;
+			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+		}
+		else
+		{
+			imageView_->setImage(uCvMat2QImage(left));
+		}
+		sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows));
+	}
+	if(!depthOrRight.empty())
+	{
+		imageView_->setImageDepth(depthOrRight);
+		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
+	}
+	imageSizeLabel_->setText(sizes);
+
+	if(!depthOrRight.empty() &&
+	   ((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
+	{
+		if(showCloudCheckbox_->isChecked())
+		{
+			if(!left.empty() && !depthOrRight.empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				if(data.imageRaw().empty())
+				{
+					if(!data.stereoCameraModels().empty())
+					{
+						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+					}
+					else
+					{
+						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+					}
+					
+				}
+				else
+				{
+					cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+				}
+			}
+			else if(!depthOrRight.empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				if(data.depthOrRightRaw().empty())
+				{
+					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
+				}
+				else
+				{
+					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
+				}
+			}
+
+			// Add landmarks to 3D Map view
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			cloudView_->removeAllCoordinates("landmark_");
+#endif
+			cloudView_->removeAllTexts();
+			if(!detections.empty())
+			{
+				for(std::map<int, MarkerInfo>::const_iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+				{
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+					cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
+#endif
+					std::string num = uNumber2Str(iter->first);
+					cloudView_->addOrUpdateText(
+							std::string("landmark_str_") + num,
+							num,
+							iter->second.pose(),
+							0.05,
+							Qt::yellow);
+				}
+			}
+		}
+	}
+
+	if(!scan.isEmpty())
+	{
+		showScanCheckbox_->setEnabled(true);
+		if(showScanCheckbox_->isChecked())
+		{
+			if(scan.hasNormals())
+			{
+				if(scan.hasIntensity())
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+				else if(scan.hasRGB())
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+				else
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+			}
+			else if(scan.hasIntensity())
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+			else if(scan.hasRGB())
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+			else
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+		}
+	}
+
+	cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
+						   (showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
+	if(cloudView_->isVisible())
+	{
+		cloudView_->refreshView();
+	}
+	if(cloudView_->getAddedClouds().contains("cloud"))
+	{
+		cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
+	}
+	if(cloudView_->getAddedClouds().contains("scan"))
+	{
+		cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
+	}
+
+	processingImages_ = false;
+}
+
+bool CameraViewer::handleEvent(UEvent * event)
+{
+	if(!pause_->isChecked())
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeData)
+			{
+				if(camEvent->data().isValid())
+				{
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+					{
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(rtabmap::SensorData, camEvent->data()));
+					}
+				}
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/CloudViewerCellPicker.cpp

@@ -0,0 +1,387 @@
+/*
+ * CloudViewerCellPicker.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include <vtkImageData.h>
+#include <vtkRenderer.h>
+#include <vtkAbstractPicker.h>
+#include <vtkPicker.h>
+#include <vtkAbstractCellLocator.h>
+#include <vtkIdList.h>
+#include <vtkCellPicker.h>
+#include <vtkLODProp3D.h>
+#include <vtkMapper.h>
+#include <vtkGenericCell.h>
+#include <vtkMath.h>
+#include <vtkTexture.h>
+#include <vtkObjectFactory.h>
+#include <vtkSmartPointer.h>
+#include <vtkPoints.h>
+#include <vtkProperty.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerCellPicker);
+
+CloudViewerCellPicker::CloudViewerCellPicker()
+{
+	cell_ = vtkGenericCell::New();
+	pointIds_ = vtkIdList::New();
+}
+
+CloudViewerCellPicker::~CloudViewerCellPicker()
+{
+	cell_->Delete();
+	pointIds_->Delete();
+}
+
+double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
+		const double p2[3],
+		double t1, double t2,
+		double tol,
+		vtkProp3D *prop,
+		vtkMapper *mapper)
+{
+	// This code was taken from the original CellPicker with almost no
+	// modification except for the locator and texture additions.
+
+	// Intersect each cell with ray.  Keep track of one closest to
+	// the eye (within the tolerance tol) and within the clipping range).
+	// Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
+	// keep track of the cell with the best pick based on parametric
+	// coordinate (pick the minimum, maximum parametric distance). This
+	// breaks ties in a reasonable way when cells are the same distance
+	// from the eye (like cells laying on a 2D plane).
+
+	vtkDataSet *data = mapper->GetInput();
+	double tMin = VTK_DOUBLE_MAX;
+	double minPCoords[3];
+	double pDistMin = VTK_DOUBLE_MAX;
+	vtkIdType minCellId = -1;
+	int minSubId = -1;
+	double minXYZ[3];
+	minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
+	double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
+	vtkMath::Normalize(ray);
+	vtkActor * actor = vtkActor::SafeDownCast(prop);
+
+	// Polydata has no 3D cells
+	int isPolyData = data->IsA("vtkPolyData");
+
+	vtkCollectionSimpleIterator iter;
+	vtkAbstractCellLocator *locator = 0;
+	this->Locators->InitTraversal(iter);
+	while ( (locator = static_cast<vtkAbstractCellLocator *>(
+			this->Locators->GetNextItemAsObject(iter))) )
+	{
+		if (locator->GetDataSet() == data)
+		{
+			break;
+		}
+	}
+
+	// Make a new p1 and p2 using the clipped t1 and t2
+	double q1[3], q2[3];
+	q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
+	q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
+	if (t1 != 0.0 || t2 != 1.0)
+	{
+		for (int j = 0; j < 3; j++)
+		{
+			q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
+			q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
+		}
+	}
+
+	// Use the locator if one exists for this data
+	if (locator)
+	{
+		vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+		vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();
+
+		locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
+		for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+		{
+			double intersection[3];
+			intersectPoints->GetPoint(i, intersection);
+		}
+
+		if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
+				minPCoords, minSubId, minCellId,
+				this->cell_))
+		{
+			return VTK_DOUBLE_MAX;
+		}
+
+		// Stretch tMin out to the original range
+		if (t1 != 0.0 || t2 != 1.0)
+		{
+			tMin = t1*(1.0 - tMin) + t2*tMin;
+		}
+
+		// If cell is a strip, then replace cell with a sub-cell
+		this->SubCellFromCell(this->cell_, minSubId);
+	}
+	else
+	{
+		vtkIdList *pointIds = this->pointIds_;
+		vtkIdType numCells = data->GetNumberOfCells();
+
+		for (vtkIdType cellId = 0; cellId < numCells; cellId++)
+		{
+			double t;
+			double x[3];
+			double pcoords[3];
+			pcoords[0] = pcoords[1] = pcoords[2] = 0;
+			int newSubId = -1;
+			int numSubIds = 1;
+
+			// If it is a strip, we need to iterate over the subIds
+			int cellType = data->GetCellType(cellId);
+			int useSubCells = this->HasSubCells(cellType);
+			if (useSubCells)
+			{
+				// Get the pointIds for the strip and the length of the strip
+				data->GetCellPoints(cellId, pointIds);
+				numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
+			}
+
+			// This will only loop once unless we need to deal with a strip
+			for (int subId = 0; subId < numSubIds; subId++)
+			{
+				if (useSubCells)
+				{
+					// Get a sub-cell from a the strip
+					this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
+				}
+				else
+				{
+					data->GetCell(cellId, this->cell_);
+				}
+
+				int cellPicked = 0;
+				if (isPolyData)
+				{
+					// Polydata can always be picked with original endpoints
+					cellPicked = this->cell_->IntersectWithLine(
+							const_cast<double *>(p1), const_cast<double *>(p2),
+							tol, t, x, pcoords, newSubId);
+				}
+				else
+				{
+					// Any 3D cells need to be intersected with a line segment that
+					// has been clipped with the clipping planes, in case one end is
+					// actually inside the cell.
+					cellPicked = this->cell_->IntersectWithLine(
+							q1, q2, tol, t, x, pcoords, newSubId);
+
+					// Stretch t out to the original range
+					if (t1 != 0.0 || t2 != 1.0)
+					{
+						t = t1*(1.0 - t) + t2*t;
+					}
+				}
+
+				if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
+				{
+					double pDist = this->cell_->GetParametricDistance(pcoords);
+					if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
+					{
+////////////////////////////////////////////////////////////////////////////////////
+// BEGIN: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+						bool visible = true;
+						if(actor->GetProperty()->GetBackfaceCulling() ||
+						   actor->GetProperty()->GetFrontfaceCulling())
+						{
+							// Get the cell weights
+							vtkIdType numPoints = this->cell_->GetNumberOfPoints();
+							double *weights = new double[numPoints];
+							for (vtkIdType i = 0; i < numPoints; i++)
+							{
+								weights[i] = 0;
+							}
+
+							// Get the interpolation weights (point is thrown away)
+							double point[3] = {0.0,0.0,0.0};
+							this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+							double normal[3] = {0.0,0.0,0.0};
+
+							if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
+							{
+								if(actor->GetProperty()->GetBackfaceCulling())
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
+								}
+								else
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
+								}
+							}
+							delete [] weights;
+						}
+						if(visible)
+						{
+							tMin = t;
+							pDistMin = pDist;
+							// save all of these
+							minCellId = cellId;
+							minSubId = newSubId;
+							if (useSubCells)
+							{
+								minSubId = subId;
+							}
+							for (int k = 0; k < 3; k++)
+							{
+								minXYZ[k] = x[k];
+								minPCoords[k] = pcoords[k];
+							}
+						}
+////////////////////////////////////////////////////////////////////////////////////
+// END: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+					} // for all subIds
+				} // if minimum, maximum
+			} // if a close cell
+		} // for all cells
+	}
+
+	// Do this if a cell was intersected
+	if (minCellId >= 0 && tMin < this->GlobalTMin)
+	{
+		this->ResetPickInfo();
+
+		// Get the cell, convert to triangle if it is a strip
+		vtkGenericCell *cell = this->cell_;
+
+		// If we used a locator, we already have the picked cell
+		if (!locator)
+		{
+			int cellType = data->GetCellType(minCellId);
+
+			if (this->HasSubCells(cellType))
+			{
+				data->GetCellPoints(minCellId, this->pointIds_);
+				this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
+			}
+			else
+			{
+				data->GetCell(minCellId, cell);
+			}
+		}
+
+		// Get the cell weights
+		vtkIdType numPoints = cell->GetNumberOfPoints();
+		double *weights = new double[numPoints];
+		for (vtkIdType i = 0; i < numPoints; i++)
+		{
+			weights[i] = 0;
+		}
+
+		// Get the interpolation weights (point is thrown away)
+		double point[3];
+		cell->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+		this->Mapper = mapper;
+
+		// Get the texture from the actor or the LOD
+		vtkActor *actor = 0;
+		vtkLODProp3D *lodActor = 0;
+		if ( (actor = vtkActor::SafeDownCast(prop)) )
+		{
+			this->Texture = actor->GetTexture();
+		}
+		else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
+		{
+			int lodId = lodActor->GetPickLODID();
+			lodActor->GetLODTexture(lodId, &this->Texture);
+		}
+
+		if (this->PickTextureData && this->Texture)
+		{
+			// Return the texture's image data to the user
+			vtkImageData *image = this->Texture->GetInput();
+			this->DataSet = image;
+
+			// Get and check the image dimensions
+			int extent[6];
+			image->GetExtent(extent);
+			int dimensionsAreValid = 1;
+			int dimensions[3];
+			for (int i = 0; i < 3; i++)
+			{
+				dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
+				dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
+			}
+
+			// Use the texture coord to set the information
+			double tcoord[3];
+			if (dimensionsAreValid &&
+					this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
+			{
+				// Take the border into account when computing coordinates
+				double x[3];
+				x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
+				x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
+				x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;
+
+				this->SetImageDataPickInfo(x, extent);
+			}
+		}
+		else
+		{
+			// Return the polydata to the user
+			this->DataSet = data;
+			this->CellId = minCellId;
+			this->SubId = minSubId;
+			this->PCoords[0] = minPCoords[0];
+			this->PCoords[1] = minPCoords[1];
+			this->PCoords[2] = minPCoords[2];
+
+			// Find the point with the maximum weight
+			double maxWeight = 0;
+			vtkIdType iMaxWeight = -1;
+			for (vtkIdType i = 0; i < numPoints; i++)
+			{
+				if (weights[i] > maxWeight)
+				{
+					iMaxWeight = i;
+				}
+			}
+
+			// If maximum weight is found, use it to get the PointId
+			if (iMaxWeight != -1)
+			{
+				this->PointId = cell->PointIds->GetId(iMaxWeight);
+			}
+		}
+
+		// Set the mapper position
+		this->MapperPosition[0] = minXYZ[0];
+		this->MapperPosition[1] = minXYZ[1];
+		this->MapperPosition[2] = minXYZ[2];
+
+		// Compute the normal
+		if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
+		{
+			// By default, the normal points back along view ray
+			this->MapperNormal[0] = p1[0] - p2[0];
+			this->MapperNormal[1] = p1[1] - p2[1];
+			this->MapperNormal[2] = p1[2] - p2[2];
+			vtkMath::Normalize(this->MapperNormal);
+		}
+
+		delete [] weights;
+	}
+
+	return tMin;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/CloudViewerInteractorStyle.cpp

@@ -0,0 +1,341 @@
+/*
+ * CloudViewerInteractorStyl.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UMath.h"
+
+#include <vtkRenderer.h>
+#include <vtkRenderWindow.h>
+#include <vtkObjectFactory.h>
+#include <vtkOBBTree.h>
+#include <vtkCamera.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerInteractorStyle);
+
+CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
+	pcl::visualization::PCLVisualizerInteractorStyle(),
+	viewer_(0),
+	NumberOfClicks(0),
+	ResetPixelDistance(0),
+	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	orthoMode_(false)
+{
+	PreviousPosition[0] = PreviousPosition[1] = 0;
+	PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
+
+	this->MotionFactor = 5;
+}
+
+void CloudViewerInteractorStyle::Rotate()
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkRenderWindowInteractor *rwi = this->Interactor;
+
+	int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
+	int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
+
+	int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
+
+	double delta_elevation = -20.0 / size[1];
+	double delta_azimuth = -20.0 / size[0];
+
+	double rxf = dx * delta_azimuth * this->MotionFactor;
+	double ryf = dy * delta_elevation * this->MotionFactor;
+
+	vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+	UASSERT(camera);
+	if(!orthoMode_)
+	{
+		camera->Azimuth(rxf);
+		camera->Elevation(ryf);
+		camera->OrthogonalizeViewUp();
+	}
+	else
+	{
+		camera->Roll(-rxf);
+	}
+
+	if (this->AutoAdjustCameraClippingRange)
+	{
+		this->CurrentRenderer->ResetCameraClippingRange();
+	}
+
+	if (rwi->GetLightFollowCamera())
+	{
+		this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+	}
+
+	//rwi->Render();
+}
+
+void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
+	UASSERT(camera);
+	camera->SetParallelProjection (enabled);
+	if(enabled)
+	{
+		double x,y,z;
+		camera->GetFocalPoint(x, y, z);
+		camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
+		camera->SetViewUp(1, 0, 0);
+	}
+	CurrentRenderer->SetActiveCamera (camera);
+	orthoMode_ = enabled;
+}
+
+void CloudViewerInteractorStyle::OnMouseMove()
+{
+	if(this->CurrentRenderer &&
+		this->CurrentRenderer->GetLayer() == 1 &&
+		this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
+		viewer_ &&
+		viewer_->getLocators().size())
+	{
+		CloudViewerCellPicker * cellPicker = dynamic_cast<CloudViewerCellPicker*>(this->Interactor->GetPicker());
+		if(cellPicker)
+		{
+			int pickPosition[2];
+			this->GetInteractor()->GetEventPosition(pickPosition);
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+
+				UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points_alt");
+				pointsHolder_->resize(2);
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
+				pointsHolder_->at(0) = pt;
+
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+
+				// Intersect the locator with the line
+				double length = 5.0;
+				double pickedNormal[3];
+				cellPicker->GetPickNormal(pickedNormal);
+				double lineP0[3] = {picked[0], picked[1], picked[2]};
+				double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
+				vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+
+				viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
+
+				// Display list of intersections
+				double intersection[3];
+				double previous[3] = {picked[0], picked[1], picked[2]};
+				for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+				{
+					intersectPoints->GetPoint(i, intersection);
+
+					Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
+					float n = v.norm();
+					if(n  > 0.01f)
+					{
+						v/=n;
+						v *= n/2.0f;
+						pt.r = 125;
+						pt.g = 125;
+						pt.b = 125;
+						pt.x = intersection[0];
+						pt.y = intersection[1];
+						pt.z = intersection[2];
+						pointsHolder_->at(1) = pt;
+						viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
+								Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
+								textSize,
+								Qt::gray);
+						viewer_->addOrUpdateLine("interactor_ray_alt",
+								Transform(previous[0], previous[1], previous[2], 0, 0, 0),
+								Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
+								Qt::gray);
+
+						previous[0] = intersection[0];
+						previous[1] = intersection[1];
+						previous[2] = intersection[2];
+						break;
+					}
+				}
+				viewer_->addCloud("interactor_points_alt", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points_alt", 15);
+				viewer_->setCloudOpacity("interactor_points_alt", 0.5);
+			}
+		}
+	}
+	// Forward events
+	PCLVisualizerInteractorStyle::OnMouseMove();
+}
+
+void CloudViewerInteractorStyle::OnLeftButtonDown()
+{
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
+	if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
+	{
+		this->NumberOfClicks++;
+		int pickPosition[2];
+		this->GetInteractor()->GetEventPosition(pickPosition);
+		int xdist = pickPosition[0] - this->PreviousPosition[0];
+		int ydist = pickPosition[1] - this->PreviousPosition[1];
+
+		this->PreviousPosition[0] = pickPosition[0];
+		this->PreviousPosition[1] = pickPosition[1];
+
+		int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
+
+		// Reset numClicks - If mouse moved further than resetPixelDistance
+		if(moveDistance > this->ResetPixelDistance)
+		{
+			this->NumberOfClicks = 1;
+		}
+
+		if(this->NumberOfClicks >= 2)
+		{
+			this->NumberOfClicks = 0;
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result && this->GetInteractor()->GetControlKey()==0)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+				UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+				UASSERT(camera);
+				double position[3];
+				double focal[3];
+				camera->GetPosition(position[0], position[1], position[2]);
+				camera->GetFocalPoint(focal[0], focal[1], focal[2]);
+				//camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
+				camera->SetFocalPoint (picked[0], picked[1], picked[2]);
+				camera->OrthogonalizeViewUp();
+
+				if (this->AutoAdjustCameraClippingRange)
+				{
+					this->CurrentRenderer->ResetCameraClippingRange();
+				}
+
+				if (this->Interactor->GetLightFollowCamera())
+				{
+					this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+				}
+			}
+			else if(viewer_)
+			{
+				viewer_->removeText("interactor_pose");
+				viewer_->removeLine("interactor_line");
+				viewer_->removeCloud("interactor_points");
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
+				viewer_->removeCloud("interactor_points_alt");
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+				PreviousMeasure[0] = 0.0f;
+				PreviousMeasure[1] = 0.0f;
+				PreviousMeasure[2] = 0.0f;
+			}
+		}
+		else if(this->GetInteractor()->GetControlKey() && viewer_)
+		{
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+
+				UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points");
+				pointsHolder_->clear();
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
+				pointsHolder_->push_back(pt);
+
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
+
+				if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
+					viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
+				{
+					viewer_->addOrUpdateLine("interactor_line",
+							Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
+							Transform(picked[0], picked[1], picked[2], 0, 0, 0),
+							Qt::red);
+					pt.x = PreviousMeasure[0];
+					pt.y = PreviousMeasure[1];
+					pt.z = PreviousMeasure[2];
+					pointsHolder_->push_back(pt);
+
+					Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
+					float n = v.norm();
+					v/=n;
+					v *= n/2.0f;
+					viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
+							Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
+							textSize,
+							Qt::red);
+				}
+				else
+				{
+					viewer_->removeText("interactor_pose");
+					viewer_->removeLine("interactor_line");
+				}
+				PreviousMeasure[0] = picked[0];
+				PreviousMeasure[1] = picked[1];
+				PreviousMeasure[2] = picked[2];
+
+				viewer_->addCloud("interactor_points", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points", 15);
+				viewer_->setCloudOpacity("interactor_points", 0.5);
+			}
+		}
+	}
+
+	// Forward events
+	PCLVisualizerInteractorStyle::OnLeftButtonDown();
+}
+
+void CloudViewerInteractorStyle::OnRightButtonDown()
+{
+	// ignore
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/ConsoleWidget.cpp

@@ -0,0 +1,161 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ConsoleWidget.h"
+#include "ui_consoleWidget.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <QMessageBox>
+#include <QtGui/QTextCursor>
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+ConsoleWidget::ConsoleWidget(QWidget * parent) :
+	QWidget(parent)
+{
+	_ui = new Ui_consoleWidget();
+	_ui->setupUi(this);
+	UEventsManager::addHandler(this);
+	_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
+	_textCursor = new QTextCursor(_ui->textEdit->document());
+	_ui->textEdit->setFontPointSize(10);
+	QPalette p(_ui->textEdit->palette());
+	p.setColor(QPalette::Base, Qt::black);
+	_ui->textEdit->setPalette(p);
+	_errorMessage = new QMessageBox(QMessageBox::Critical, tr("Fatal error occurred"), "", QMessageBox::Ok, this);
+	_errorMessageMutex.lock();
+	_time.start();
+	_timer.setSingleShot(true);
+	connect(_ui->pushButton_clear, SIGNAL(clicked()), _ui->textEdit, SLOT(clear()));
+	connect(_ui->spinBox_lines, SIGNAL(valueChanged(int)), this, SLOT(updateTextEditBufferSize()));
+	connect(this, SIGNAL(msgReceived(const QString &, int)), this, SLOT(appendMsg(const QString &, int)));
+	connect(&_timer, SIGNAL(timeout()), this, SLOT(flushConsole()));
+}
+
+ConsoleWidget::~ConsoleWidget()
+{
+	delete _ui;
+	_errorMessageMutex.unlock();
+}
+
+bool ConsoleWidget::handleEvent(UEvent * anEvent)
+{
+	// WARNING, don't put a log message here! otherwise it could be recursively called.
+	if(anEvent->getClassName().compare("ULogEvent") == 0)
+	{
+		ULogEvent * logEvent = (ULogEvent*)anEvent;
+		_msgListMutex.lock();
+		_msgList.append(QPair<QString, int>(logEvent->getMsg().c_str(), logEvent->getCode()));
+		while(_ui->spinBox_lines->value()>0 && _msgList.size()>_ui->spinBox_lines->value())
+		{
+			_msgList.pop_front();
+		}
+		_msgListMutex.unlock();
+
+		if(_ui->spinBox_time->value()>0 && _time.restart() < _ui->spinBox_time->value())
+		{
+			if(logEvent->getCode() == ULogger::kFatal)
+			{
+				QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
+			}
+			else
+			{
+				QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, _ui->spinBox_time->value()));
+			}
+		}
+		else
+		{
+			QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
+		}
+
+		if(logEvent->getCode() == ULogger::kFatal)
+		{
+			//This thread will wait until the message box is closed...
+			// Assuming that error messages come from a different thread.
+			_errorMessageMutex.lock();
+		}
+
+	}
+	return false;
+}
+
+void ConsoleWidget::appendMsg(const QString & msg, int level)
+{
+	switch(level)
+	{
+	case 0:
+		_ui->textEdit->setTextColor(Qt::darkGreen);
+		break;
+	case 2:
+		_ui->textEdit->setTextColor(Qt::yellow);
+		break;
+	case 3:
+	case 4:
+		_ui->textEdit->setTextColor(Qt::red);
+		break;
+	default:
+		_ui->textEdit->setTextColor(Qt::white);
+		break;
+	}
+	_ui->textEdit->append(msg);
+
+	if(level == ULogger::kFatal)
+	{
+		_textCursor->endEditBlock();
+		QTextCursor cursor = _ui->textEdit->textCursor();
+		cursor.movePosition(QTextCursor::End, QTextCursor::MoveAnchor);
+		_ui->textEdit->setTextCursor(cursor);
+		//The application will exit, so warn the user.
+		_errorMessage->setText(tr("Description:\n\n%1\n\nThe application will now exit...").arg(msg));
+		_errorMessage->exec();
+		_errorMessageMutex.unlock();
+	}
+}
+
+void ConsoleWidget::flushConsole()
+{
+	_msgListMutex.lock();
+	_textCursor->beginEditBlock();
+	for(int i=0; i<_msgList.size(); ++i)
+	{
+		appendMsg(_msgList[i].first, _msgList[i].second);
+	}
+	_textCursor->endEditBlock();
+	_msgList.clear();
+	_msgListMutex.unlock();
+	QTextCursor cursor = _ui->textEdit->textCursor();
+	cursor.movePosition(QTextCursor::End, QTextCursor::MoveAnchor);
+	_ui->textEdit->setTextCursor(cursor);
+}
+
+void ConsoleWidget::updateTextEditBufferSize()
+{
+	_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
+}
+
+}

rtabmap/guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -0,0 +1,309 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/CreateSimpleCalibrationDialog.h"
+#include "ui_createSimpleCalibrationDialog.h"
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/utilite/ULogger.h"
+
+#include <QFileDialog>
+#include <QPushButton>
+#include <QMessageBox>
+
+namespace rtabmap {
+
+CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
+		const QString & savingFolder,
+		const QString & cameraName,
+		QWidget * parent) :
+	QDialog(parent),
+	savingFolder_(savingFolder),
+	cameraName_(cameraName)
+{
+	if(cameraName_.isEmpty())
+	{
+		cameraName_ = "calib";
+	}
+
+	ui_ = new Ui_createSimpleCalibrationDialog();
+	ui_->setupUi(this);
+
+	connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
+	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
+
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoView()));
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
+
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
+
+	updateStereoView();
+}
+
+CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
+{
+	delete ui_;
+}
+
+void CreateSimpleCalibrationDialog::updateStereoView()
+{
+	bool checked = ui_->checkBox_stereo->isChecked();
+	ui_->doubleSpinBox_baseline->setVisible(checked);
+	ui_->label_baseline->setVisible(checked);
+	ui_->label_right->setVisible(checked);
+	ui_->doubleSpinBox_fx_r->setVisible(checked);
+	ui_->doubleSpinBox_fy_r->setVisible(checked);
+	ui_->doubleSpinBox_cx_r->setVisible(checked);
+	ui_->doubleSpinBox_cy_r->setVisible(checked);
+	ui_->lineEdit_D_r->setVisible(checked);
+	ui_->groupBox_stereo_extrinsics->setVisible(ui_->comboBox_advanced->currentIndex() == 1 && checked);
+	ui_->label_left->setVisible(checked);
+}
+
+void CreateSimpleCalibrationDialog::updateSaveStatus()
+{
+	bool valid = false;
+	if(ui_->comboBox_advanced->currentIndex() == 0 &&
+	   ui_->doubleSpinBox_fx->value() > 0.0 &&
+	   ui_->doubleSpinBox_fy->value() > 0.0 &&
+	   ui_->spinBox_width->value() > 0 &&
+	   ui_->spinBox_height->value() > 0 &&
+	   (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
+	{
+		// basic
+		valid = true;
+	}
+	else if(ui_->comboBox_advanced->currentIndex() == 1 &&
+			 ui_->doubleSpinBox_fx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_fy_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cy_l->value() > 0.0 &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
+			 ui_->spinBox_width->value() > 0 &&
+			 ui_->spinBox_height->value() > 0 &&
+			 !ui_->lineEdit_D_l->text().isEmpty() &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
+	{
+		//advanced
+		QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString();
+
+		if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
+				(!ui_->checkBox_stereo->isChecked() || (distorsionsStrListR.size() == 4 || distorsionsStrListR.size() == 5 || distorsionsStrListR.size() == 8)) &&
+				(!ui_->checkBox_stereo->isChecked() || (!RT.empty() && Transform::canParseString(RT))))
+		{
+			valid = true;
+		}
+	}
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
+}
+
+void CreateSimpleCalibrationDialog::saveCalibration()
+{
+	QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
+	QString name = QFileInfo(filePath).baseName();
+	QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+	if(!filePath.isEmpty())
+	{
+		cameraName_ = name;
+		CameraModel modelLeft;
+		float width = ui_->spinBox_width->value();
+		float height = ui_->spinBox_height->value();
+		if(ui_->comboBox_advanced->currentIndex() == 0)
+		{
+			//basic
+			modelLeft = CameraModel(
+					name.toStdString(),
+					ui_->doubleSpinBox_fx->value(),
+					ui_->doubleSpinBox_fy->value(),
+					ui_->doubleSpinBox_cx->value(),
+					ui_->doubleSpinBox_cy->value(),
+					CameraModel::opticalRotation(),
+					0,
+					cv::Size(width, height));
+			UASSERT(modelLeft.isValidForProjection());
+		}
+		else
+		{
+			//advanced
+			cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+			K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
+			K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
+			K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
+			K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
+			cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+			cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+			K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+			QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+			UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+			cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+			bool ok;
+			for(int i=0; i<distorsionCoeffs.size(); ++i)
+			{
+				D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+				if(!ok)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing left distortion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
+					return;
+				}
+			}
+
+			modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+			UASSERT(modelLeft.isValidForRectification());
+		}
+
+		if(ui_->checkBox_stereo->isChecked())
+		{
+			StereoCameraModel stereoModel;
+			if(ui_->comboBox_advanced->currentIndex() == 0)
+			{
+				CameraModel modelRight(
+						name.toStdString(),
+						ui_->doubleSpinBox_fx->value(),
+						ui_->doubleSpinBox_fy->value(),
+						ui_->doubleSpinBox_cx->value(),
+						ui_->doubleSpinBox_cy->value(),
+						Transform::getIdentity(),
+						ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value(),
+						cv::Size(width, height));
+				UASSERT(modelRight.isValidForProjection());
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
+				UASSERT(stereoModel.isValidForProjection());
+			}
+			else if(ui_->comboBox_advanced->currentIndex() == 1)
+			{
+				CameraModel modelRight;
+				cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+				K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
+				K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
+				K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
+				K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
+				cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+				cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+				K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+				QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+				UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+				cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+				bool ok;
+				for(int i=0; i<distorsionCoeffs.size(); ++i)
+				{
+					D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+					if(!ok)
+					{
+						QMessageBox::warning(this, tr("Save"), tr("Error parsing right distortion coefficients \"%1\".").arg(ui_->lineEdit_D_r->text()));
+						return;
+					}
+				}
+
+				modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+				UASSERT(modelRight.isValidForRectification());
+
+				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString()));
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
+				if(stereoModel.baseline() < 0)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing the extrinsics \"%1\", resulting baseline (%f) is negative!").arg(ui_->lineEdit_RT->text()).arg(stereoModel.baseline()));
+					return;
+				}
+				UASSERT(stereoModel.isValidForRectification());
+			}
+
+			std::string base = (dir+QDir::separator()+name).toStdString();
+			std::string leftPath = base+"_left.yaml";
+			std::string rightPath = base+"_right.yaml";
+
+			if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
+			{
+				if(ui_->comboBox_advanced->currentIndex() == 0)
+				{
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()));
+				}
+				else
+				{
+					std::string posePath = base+"_pose.yaml";
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
+				}
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
+			}
+		}
+		else
+		{
+			if(modelLeft.save(dir.toStdString()))
+			{
+				QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
+			}
+		}
+	}
+}
+
+}

rtabmap/guilib/src/DataRecorder.cpp

@@ -0,0 +1,206 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/DataRecorder.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/SensorEvent.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <QtCore/QMetaType>
+#include <QMessageBox>
+#include <QtGui/QCloseEvent>
+#include <QLabel>
+#include <QVBoxLayout>
+
+namespace rtabmap {
+
+
+DataRecorder::DataRecorder(QWidget * parent) :
+		QWidget(parent),
+	memory_(0),
+	imageView_(new ImageView(this)),
+	label_(new QLabel(this)),
+	processingImages_(false),
+	count_(0),
+	totalSizeKB_(0)
+{
+	qRegisterMetaType<cv::Mat>("cv::Mat");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	layout->setContentsMargins(0,0,0,0);
+	layout->addWidget(imageView_);
+	layout->addWidget(label_);
+	layout->setStretch(0,1);
+	this->setLayout(layout);
+}
+bool DataRecorder::init(const QString & path, bool recordInRAM)
+{
+	UScopeMutex scope(memoryMutex_);
+	if(!memory_)
+	{
+		ParametersMap customParameters;
+		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal
+		customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); // deactivate keypoints extraction
+		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
+		customParameters.insert(ParametersPair(Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
+		customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make sure memory cleanup is done
+		customParameters.insert(ParametersPair(Parameters::kMemIntermediateNodeDataKept(), "true"));
+		if(!recordInRAM)
+		{
+			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+		}
+		memory_ = new Memory();
+		if(!memory_->init(path.toStdString(), true, customParameters))
+		{
+			delete memory_;
+			memory_ = 0;
+			UERROR("Error initializing the memory.");
+			return false;
+		}
+		path_ = path;
+		return true;
+	}
+	else
+	{
+		UERROR("Already initialized, close it first.");
+		return false;
+	}
+}
+
+void DataRecorder::closeRecorder()
+{
+	memoryMutex_.lock();
+	if(memory_)
+	{
+		delete memory_;
+		memory_ = 0;
+		UINFO("Data recorded to \"%s\".", this->path().toStdString().c_str());
+	}
+	memoryMutex_.unlock();
+	processingImages_ = false;
+	count_ = 0;
+	totalSizeKB_ = 0;
+	if(this->isVisible())
+	{
+		QMessageBox::information(this, tr("Data recorder"), tr("Data recorded to \"%1\".").arg(this->path()));
+	}
+}
+
+DataRecorder::~DataRecorder()
+{
+	this->unregisterFromEventsManager();
+	this->closeRecorder();
+}
+
+void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
+{
+	memoryMutex_.lock();
+	if(memory_)
+	{
+		if(memory_->getStMem().size() == 0 && data.id() > 0)
+		{
+			ParametersMap customParameters;
+			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
+			memory_->parseParameters(customParameters);
+		}
+
+		//save to database
+		UTimer time;
+		memory_->update(data, pose, covariance);
+		const Signature * s = memory_->getLastWorkingSignature();
+		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000;
+		memory_->cleanup();
+
+		if(++count_ % 30)
+		{
+			memory_->emptyTrash();
+		}
+		UDEBUG("Time to process a message = %f s, totalSizeKB_=%d", time.ticks(), totalSizeKB_);
+	}
+	memoryMutex_.unlock();
+}
+
+void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
+{
+	processingImages_ = true;
+	if(!image.empty()) {
+		imageView_->setImage(uCvMat2QImage(image));
+	}
+	if(!depth.empty()) {
+		imageView_->setImageDepth(depth);
+	}
+	label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
+	processingImages_ = false;
+}
+
+void DataRecorder::closeEvent(QCloseEvent* event)
+{
+	this->closeRecorder();
+	event->accept();
+}
+
+bool DataRecorder::handleEvent(UEvent * event)
+{
+	if(memory_)
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeData)
+			{
+				if(camEvent->data().isValid())
+				{
+					UINFO("Receiving rate = %f Hz", 1.0f/timer_.ticks());
+					this->addData(
+							camEvent->data(),
+							camEvent->info().odomPose,
+							camEvent->info().odomCovariance.empty()?cv::Mat::eye(6,6,CV_64FC1):camEvent->info().odomCovariance);
+
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+					{
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(cv::Mat, camEvent->data().imageRaw()),
+								Q_ARG(cv::Mat, camEvent->data().depthOrRightRaw()));
+					}
+				}
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/DepthCalibrationDialog.cpp

@@ -0,0 +1,562 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/gui/DepthCalibrationDialog.h>
+#include "ui_depthCalibrationDialog.h"
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UThread.h"
+#include "rtabmap/utilite/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+
+#include "rtabmap/core/clams/slam_calibrator.h"
+#include "rtabmap/core/clams/frame_projector.h"
+
+#include <QPushButton>
+#include <QDir>
+#include <QFileInfo>
+#include <QUrl>
+#include <QtGui/QDesktopServices>
+#include <QMessageBox>
+#include <QFileDialog>
+
+namespace rtabmap {
+
+DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
+			QDialog(parent),
+			_canceled(false),
+			_model(0)
+{
+	_ui = new Ui_DepthCalibrationDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveModel()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(accept()));
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setText("Calibrate");
+
+	restoreDefaults();
+
+	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_coneRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_coneStdDevThresh, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_laserScan, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	connect(_ui->spinBox_bin_width, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_bin_height, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_bin_depth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_smoothing, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_maxDepthModel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setFocus();
+
+	_progressDialog = new ProgressDialog(this);
+	_progressDialog->setVisible(false);
+	_progressDialog->setAutoClose(true, 2);
+	_progressDialog->setMinimumWidth(600);
+	_progressDialog->setCancelButtonVisible(true);
+
+	connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
+}
+
+DepthCalibrationDialog::~DepthCalibrationDialog()
+{
+	delete _ui;
+	delete _model;
+}
+
+void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+
+	settings.setValue("decimation", _ui->spinBox_decimation->value());
+	settings.setValue("max_depth", _ui->doubleSpinBox_maxDepth->value());
+	settings.setValue("min_depth", _ui->doubleSpinBox_minDepth->value());
+	settings.setValue("voxel",_ui->doubleSpinBox_voxelSize->value());
+	settings.setValue("cone_radius",_ui->doubleSpinBox_coneRadius->value());
+	settings.setValue("cone_stddev_thresh",_ui->doubleSpinBox_coneStdDevThresh->value());
+	settings.setValue("laser_scan",_ui->checkBox_laserScan->isChecked());
+
+	settings.setValue("bin_width",_ui->spinBox_bin_width->value());
+	settings.setValue("bin_height",_ui->spinBox_bin_height->value());
+	settings.setValue("bin_depth",_ui->doubleSpinBox_bin_depth->value());
+	settings.setValue("smoothing",_ui->spinBox_smoothing->value());
+	settings.setValue("max_model_depth",_ui->doubleSpinBox_maxDepthModel->value());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void DepthCalibrationDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+
+	_ui->spinBox_decimation->setValue(settings.value("decimation", _ui->spinBox_decimation->value()).toInt());
+	_ui->doubleSpinBox_maxDepth->setValue(settings.value("max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
+	_ui->doubleSpinBox_minDepth->setValue(settings.value("min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
+	_ui->doubleSpinBox_voxelSize->setValue(settings.value("voxel", _ui->doubleSpinBox_voxelSize->value()).toDouble());
+	_ui->doubleSpinBox_coneRadius->setValue(settings.value("cone_radius", _ui->doubleSpinBox_coneRadius->value()).toDouble());
+	_ui->doubleSpinBox_coneStdDevThresh->setValue(settings.value("cone_stddev_thresh", _ui->doubleSpinBox_coneStdDevThresh->value()).toDouble());
+	_ui->checkBox_laserScan->setChecked(settings.value("laser_scan", _ui->checkBox_laserScan->isChecked()).toBool());
+
+	_ui->spinBox_bin_width->setValue(settings.value("bin_width", _ui->spinBox_bin_width->value()).toInt());
+	_ui->spinBox_bin_height->setValue(settings.value("bin_height", _ui->spinBox_bin_height->value()).toInt());
+	_ui->doubleSpinBox_bin_depth->setValue(settings.value("bin_depth", _ui->doubleSpinBox_bin_depth->value()).toDouble());
+	_ui->spinBox_smoothing->setValue(settings.value("smoothing", _ui->spinBox_smoothing->value()).toInt());
+	_ui->doubleSpinBox_maxDepthModel->setValue(settings.value("max_model_depth", _ui->doubleSpinBox_maxDepthModel->value()).toDouble());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void DepthCalibrationDialog::restoreDefaults()
+{
+	_ui->spinBox_decimation->setValue(1);
+	_ui->doubleSpinBox_maxDepth->setValue(3.5);
+	_ui->doubleSpinBox_minDepth->setValue(0);
+	_ui->doubleSpinBox_voxelSize->setValue(0.01);
+	_ui->doubleSpinBox_coneRadius->setValue(0.02);
+	_ui->doubleSpinBox_coneStdDevThresh->setValue(0.1); // 0.03
+	_ui->checkBox_laserScan->setChecked(false);
+	_ui->checkBox_resetModel->setChecked(true);
+
+	_ui->spinBox_bin_width->setValue(8);
+	_ui->spinBox_bin_height->setValue(6);
+	if(_imageSize.width > 0 && _imageSize.height > 0)
+	{
+		size_t bin_width, bin_height;
+		clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
+		_ui->spinBox_bin_width->setValue(bin_width);
+		_ui->spinBox_bin_height->setValue(bin_height);
+	}
+	_ui->doubleSpinBox_bin_depth->setValue(2.0),
+	_ui->spinBox_smoothing->setValue(1);
+	_ui->doubleSpinBox_maxDepthModel->setValue(10.0);
+}
+
+void DepthCalibrationDialog::saveModel()
+{
+	if(_model && _model->getTrainingSamples())
+	{
+		QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin *.txt)"));
+		if(!path.isEmpty())
+		{
+			//
+			// Save depth calibration
+			//
+			cv::Mat results = _model->visualize(ULogger::level() == ULogger::kDebug?_workingDirectory.toStdString():"");
+			_model->save(path.toStdString());
+
+			if(!results.empty())
+			{
+				QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive).replace(".txt", ".png", Qt::CaseInsensitive);
+				cv::imwrite(name.toStdString(), results);
+				QDesktopServices::openUrl(QUrl::fromLocalFile(name));
+			}
+
+			QMessageBox::information(this, tr("Depth Calibration"), tr("Distortion model saved to \"%1\"!").arg(path));
+		}
+	}
+}
+
+void DepthCalibrationDialog::cancel()
+{
+	_canceled = true;
+	_progressDialog->appendText(tr("Canceled!"));
+}
+
+void DepthCalibrationDialog::calibrate(
+		const std::map<int, Transform> & poses,
+			const QMap<int, Signature> & cachedSignatures,
+			const QString & workingDirectory,
+			const ParametersMap & parameters)
+{
+	_canceled = false;
+	_workingDirectory = workingDirectory;
+	_ui->buttonBox->button(QDialogButtonBox::Save)->setEnabled(_model && _model->getTrainingSamples()>0);
+	if(_model)
+	{
+		_ui->label_trainingSamples->setNum((int)_model->getTrainingSamples());
+	}
+
+	_ui->label_width->setText("NA");
+	_ui->label_height->setText("NA");
+	_imageSize = cv::Size();
+	CameraModel model;
+	if(cachedSignatures.size())
+	{
+		const Signature & s = cachedSignatures.begin().value();
+		const SensorData & data = s.sensorData();
+		cv::Mat depth;
+		data.uncompressDataConst(0, &depth);
+		if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
+		{
+			// use depth image size
+			_imageSize = depth.size();
+			_ui->label_width->setNum(_imageSize.width);
+			_ui->label_height->setNum(_imageSize.height);
+
+			if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
+				_imageSize.height % _ui->spinBox_bin_height->value() != 0)
+			{
+				size_t bin_width, bin_height;
+				clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
+				_ui->spinBox_bin_width->setValue(bin_width);
+				_ui->spinBox_bin_height->setValue(bin_height);
+			}
+		}
+		else if(data.cameraModels().size() > 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
+			return;
+		}
+		else if(data.cameraModels().size() != 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
+			return;
+		}
+		else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
+			return;
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
+			return;
+		}
+	}
+	else
+	{
+		QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
+		return;
+	}
+
+	if(this->exec() == QDialog::Accepted)
+	{
+		if(_model && _ui->checkBox_resetModel->isChecked())
+		{
+			delete _model;
+			_model = 0;
+		}
+
+		if(_ui->doubleSpinBox_maxDepthModel->value() < _ui->doubleSpinBox_bin_depth->value())
+		{
+			QMessageBox::warning(this, tr("Wrong parameter"), tr("Maximum model depth should be higher than bin depth, setting to bin depth x5."));
+			_ui->doubleSpinBox_maxDepthModel->setValue(_ui->doubleSpinBox_bin_depth->value() * 5.0);
+		}
+
+		_progressDialog->setMaximumSteps(poses.size()*2 + 3);
+		if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+		{
+			_progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+1);
+		}
+		_progressDialog->resetProgress();
+		_progressDialog->show();
+
+		std::map<int, rtabmap::SensorData> sequence;
+
+		// Create the map
+		pcl::PointCloud<pcl::PointXYZ>::Ptr  map(new pcl::PointCloud<pcl::PointXYZ>);
+		int index=1;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
+		{
+			int points = 0;
+			if(!iter->second.isNull())
+			{
+				pcl::IndicesPtr indices(new std::vector<int>);
+				if(cachedSignatures.contains(iter->first))
+				{
+					const Signature & s = cachedSignatures.find(iter->first).value();
+					SensorData data = s.sensorData();
+
+					cv::Mat  depth;
+					LaserScan laserScan;
+					data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
+					if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
+					{
+						UASSERT(iter->first == data.id());
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+						if(_ui->checkBox_laserScan->isChecked())
+						{
+							cloud = util3d::laserScanToPointCloud(laserScan);
+							indices->resize(cloud->size());
+							for(unsigned int i=0; i<indices->size(); ++i)
+							{
+								indices->at(i) = i;
+							}
+						}
+						else
+						{
+							cloud = util3d::cloudFromSensorData(
+									data,
+									_ui->spinBox_decimation->value(),
+									_ui->doubleSpinBox_maxDepth->value(),
+									_ui->doubleSpinBox_minDepth->value(),
+									indices.get(),
+									parameters);
+						}
+
+						if(indices->size())
+						{
+							if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+							{
+								cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
+							}
+
+							cloud = util3d::transformPointCloud(cloud, iter->second);
+
+							points+=cloud->size();
+
+							*map += *cloud;
+
+							sequence.insert(std::make_pair(iter->first, data));
+
+							cv::Size size = depth.size();
+							if(_model &&
+								(_model->getWidth()!=size.width ||
+								 _model->getHeight()!=size.height))
+							{
+								QString msg = tr("Depth images (%1x%2) in the map don't have the "
+										   "same size then in the current model (%3x%4). You may want "
+										   "to check \"Reset previous model\" before trying again.")
+										   .arg(size.width).arg(size.height)
+										   .arg(_model->getWidth()).arg(_model->getHeight());
+								QMessageBox::warning(this, tr("Depth Calibration"), msg);
+								_progressDialog->appendText(msg, Qt::darkRed);
+								_progressDialog->setAutoClose(false);
+								return;
+							}
+						}
+					}
+					else
+					{
+						_progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
+						_progressDialog->setAutoClose(false);
+					}
+				}
+				else
+				{
+					UERROR("Cloud %d not found in cache!", iter->first);
+				}
+			}
+			else
+			{
+				UERROR("transform is null!?");
+			}
+
+			if(points>0)
+			{
+				_progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
+						.arg(iter->first).arg(points).arg(index).arg(poses.size()));
+			}
+			else
+			{
+				_progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(index).arg(poses.size()));
+			}
+			_progressDialog->incrementStep();
+			QApplication::processEvents();
+		}
+
+		if(!_canceled && map->size() && sequence.size())
+		{
+			if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+			{
+				_progressDialog->appendText(tr("Voxel filtering (%1 m) of the merged point cloud (%2 points)")
+						.arg(_ui->doubleSpinBox_voxelSize->value())
+						.arg(map->size()));
+				QApplication::processEvents();
+				QApplication::processEvents();
+
+				map = util3d::voxelize(map, _ui->doubleSpinBox_voxelSize->value());
+				_progressDialog->incrementStep();
+			}
+
+			if(ULogger::level() == ULogger::kDebug)
+			{
+				//
+				// Show 3D map with frustums
+				//
+				QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
+				window->setAttribute(Qt::WA_DeleteOnClose, true);
+				window->setWindowTitle(tr("Map"));
+				window->setMinimumWidth(800);
+				window->setMinimumHeight(600);
+
+				CloudViewer * viewer = new CloudViewer(window);
+				viewer->setCameraLockZ(false);
+				viewer->setFrustumShown(true);
+
+				QVBoxLayout *layout = new QVBoxLayout();
+				layout->addWidget(viewer);
+				window->setLayout(layout);
+				connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
+
+				window->show();
+
+				uSleep(500);
+
+				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
+				_progressDialog->incrementStep();
+				viewer->addCloud("map", map);
+				for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
+				{
+					Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
+					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
+				}
+				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
+
+				viewer->update();
+			}
+
+			_progressDialog->appendText(tr("CLAMS depth calibration..."));
+			QApplication::processEvents();
+			QApplication::processEvents();
+
+			QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
+			dialog->setAttribute(Qt::WA_DeleteOnClose, true);
+			dialog->setWindowTitle(tr("Original/Map"));
+			dialog->setMinimumWidth(_imageSize.width);
+			ImageView * imageView1 = new ImageView(dialog);
+			imageView1->setMinimumSize(320, 240);
+			ImageView * imageView2 = new ImageView(dialog);
+			imageView2->setMinimumSize(320, 240);
+			QVBoxLayout * vlayout = new QVBoxLayout();
+			vlayout->setContentsMargins(0,0,0,0);
+			vlayout->addWidget(imageView1, 1);
+			vlayout->addWidget(imageView2, 1);
+			dialog->setLayout(vlayout);
+			if(ULogger::level() == ULogger::kDebug)
+			{
+				dialog->show();
+			}
+
+			//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
+			const cv::Size & imageSize = _imageSize;
+			if(_model == 0)
+			{
+				size_t bin_width = _ui->spinBox_bin_width->value();
+				size_t bin_height = _ui->spinBox_bin_height->value();
+				if(imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
+				   imageSize.height % _ui->spinBox_bin_height->value() != 0)
+				{
+					size_t bin_width, bin_height;
+					clams::DiscreteDepthDistortionModel::getBinSize(imageSize.width, imageSize.height, bin_width, bin_height);
+					_ui->spinBox_bin_width->setValue(bin_width);
+					_ui->spinBox_bin_height->setValue(bin_height);
+				}
+				_model = new clams::DiscreteDepthDistortionModel(
+						imageSize.width,
+						imageSize.height,
+						bin_width,
+						bin_height,
+						_ui->doubleSpinBox_bin_depth->value(),
+						_ui->spinBox_smoothing->value(),
+						_ui->doubleSpinBox_maxDepthModel->value());
+			}
+			UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);
+
+			// -- For all selected frames, accumulate training examples
+			//    in the distortion model.
+			size_t counts;
+			index = 0;
+			for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter != poses.end() && !_canceled; ++iter)
+			{
+			  size_t idx = iter->first;
+			  std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
+			  if(ster!=sequence.end())
+			  {
+				  cv::Mat depthImage;
+				  ster->second.uncompressDataConst(0, &depthImage);
+
+				  if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
+				  {
+					  cv::Mat mapDepth;
+					  CameraModel model = ster->second.cameraModels()[0];
+					  if(model.imageWidth() != depthImage.cols)
+					  {
+						  UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
+						  model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
+					  }
+					  clams::FrameProjector projector(model);
+					  mapDepth = projector.estimateMapDepth(
+							  map,
+							  iter->second.inverse(),
+							  depthImage,
+							 _ui->doubleSpinBox_coneRadius->value(),
+							 _ui->doubleSpinBox_coneStdDevThresh->value());
+
+					  if(ULogger::level() == ULogger::kDebug)
+					  {
+						  imageView1->setImage(uCvMat2QImage(depthImage));
+						  imageView2->setImage(uCvMat2QImage(mapDepth));
+					  }
+
+					  counts = _model->accumulate(mapDepth, depthImage);
+					  _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
+				  }
+				 }
+			  _progressDialog->incrementStep();
+			  QApplication::processEvents();
+			}
+
+			_progressDialog->appendText(tr("CLAMS depth calibration... done!"));
+			QApplication::processEvents();
+
+			if(!_canceled)
+			{
+				this->saveModel();
+			}
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("The resulting map is empty!"));
+		}
+		_progressDialog->setValue(_progressDialog->maximumSteps());
+	}
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/EditConstraintDialog.cpp

@@ -0,0 +1,153 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/EditConstraintDialog.h"
+#include "ui_editConstraintDialog.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <iostream>
+
+#ifndef M_PI
+#define M_PI       3.14159265358979323846
+#endif
+
+namespace rtabmap {
+
+EditConstraintDialog::EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance, QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_EditConstraintDialog();
+	_ui->setupUi(this);
+
+	float x,y,z,roll,pitch,yaw;
+	constraint.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+	_ui->x->setValue(x);
+	_ui->y->setValue(y);
+	_ui->z->setValue(z);
+	_ui->roll->setValue(roll);
+	_ui->pitch->setValue(pitch);
+	_ui->yaw->setValue(yaw);
+
+	UASSERT(covariance.empty() || (covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1));
+
+	_ui->checkBox_radians->setChecked(true);
+	_ui->linear_sigma_x->setValue(covariance.empty() || covariance.at<double>(0,0)>=9999 || covariance.at<double>(0,0)<=0?0:sqrt(covariance.at<double>(0,0)));
+	_ui->linear_sigma_y->setValue(covariance.empty() || covariance.at<double>(1,1)>=9999 || covariance.at<double>(1,1)<=0?0:sqrt(covariance.at<double>(1,1)));
+	_ui->linear_sigma_z->setValue(covariance.empty() || covariance.at<double>(2,2)>=9999 || covariance.at<double>(2,2)<=0?0:sqrt(covariance.at<double>(2,2)));
+	_ui->angular_sigma_roll->setValue(covariance.empty() || covariance.at<double>(3,3)>=9999 || covariance.at<double>(3,3)<=0?0:sqrt(covariance.at<double>(3,3)));
+	_ui->angular_sigma_pitch->setValue(covariance.empty() || covariance.at<double>(4,4)>=9999 || covariance.at<double>(4,4)<=0?0:sqrt(covariance.at<double>(4,4)));
+	_ui->angular_sigma_yaw->setValue(covariance.empty() || covariance.at<double>(5,5)>=9999 || covariance.at<double>(5,5)<=0?0:sqrt(covariance.at<double>(5,5)));
+
+	connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
+}
+
+EditConstraintDialog::~EditConstraintDialog()
+{
+	delete _ui;
+}
+
+void EditConstraintDialog::setPoseGroupVisible(bool visible)
+{
+	_ui->groupBox_pose->setVisible(visible);
+}
+void EditConstraintDialog::setCovarianceGroupVisible(bool visible)
+{
+	_ui->groupBox_covariance->setVisible(visible);
+}
+
+void EditConstraintDialog::switchUnits()
+{
+	double conversion = 180.0/M_PI;
+	if(_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	QVector<QDoubleSpinBox*> boxes;
+	boxes.push_back(_ui->roll);
+	boxes.push_back(_ui->pitch);
+	boxes.push_back(_ui->yaw);
+	boxes.push_back(_ui->angular_sigma_roll);
+	boxes.push_back(_ui->angular_sigma_pitch);
+	boxes.push_back(_ui->angular_sigma_yaw);
+	for(int i=0; i<boxes.size(); ++i)
+	{
+		double value = boxes[i]->value()*conversion;
+		if(_ui->checkBox_radians->isChecked())
+		{
+			if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
+			{
+				boxes[i]->setMinimum(-M_PI);
+			}
+			boxes[i]->setMaximum(M_PI);
+			boxes[i]->setSuffix(" rad");
+			boxes[i]->setSingleStep(0.01);
+		}
+		else
+		{
+			if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
+			{
+				boxes[i]->setMinimum(-180);
+			}
+			boxes[i]->setMaximum(180);
+			boxes[i]->setSuffix(" deg");
+			boxes[i]->setSingleStep(1);
+		}
+		boxes[i]->setValue(value);
+	}
+}
+
+Transform EditConstraintDialog::getTransform() const
+{
+	double conversion = 1.0f;
+	if(!_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value()*conversion, _ui->pitch->value()*conversion, _ui->yaw->value()*conversion);
+}
+
+cv::Mat EditConstraintDialog::getCovariance() const
+{
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = _ui->linear_sigma_x->value()==0?9999:_ui->linear_sigma_x->value()*_ui->linear_sigma_x->value();
+	covariance.at<double>(1,1) = _ui->linear_sigma_y->value()==0?9999:_ui->linear_sigma_y->value()*_ui->linear_sigma_y->value();
+	covariance.at<double>(2,2) = _ui->linear_sigma_z->value()==0?9999:_ui->linear_sigma_z->value()*_ui->linear_sigma_z->value();
+	double conversion = 1.0f;
+	if(!_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	double sigma = _ui->angular_sigma_roll->value()*conversion;
+	covariance.at<double>(3,3) = sigma==0?9999:sigma*sigma;
+	sigma = _ui->angular_sigma_pitch->value()*conversion;
+	covariance.at<double>(4,4) = sigma==0?9999:sigma*sigma;
+	sigma = _ui->angular_sigma_yaw->value()*conversion;
+	covariance.at<double>(5,5) = sigma==0?9999:sigma*sigma;
+	return covariance;
+}
+
+}

rtabmap/guilib/src/EditDepthArea.cpp

@@ -0,0 +1,434 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <QWidget>
+#include <QPainter>
+#include <QMouseEvent>
+#include <QMenu>
+#include <QAction>
+#include <QActionGroup>
+#include <QInputDialog>
+
+#include "rtabmap/gui/EditDepthArea.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+EditDepthArea::EditDepthArea(QWidget *parent)
+    : QWidget(parent)
+{
+    setAttribute(Qt::WA_StaticContents);
+    modified_ = false;
+    scribbling_ = false;
+    myPenWidth_ = 10;
+    clusterError_ = 0.02;
+
+    menu_ = new QMenu(tr(""), this);
+    showRGB_ = menu_->addAction(tr("Show RGB Image"));
+    showRGB_->setCheckable(true);
+    showRGB_->setChecked(true);
+    removeCluster_ = menu_->addAction(tr("Remove Cluster"));
+    clusterErrorCluster_ = menu_->addAction(tr("Set Cluster Error"));
+	setPenWidth_ = menu_->addAction(tr("Set Pen Width..."));
+	QMenu * colorMap = menu_->addMenu("Depth color map");
+	colorMapWhiteToBlack_ = colorMap->addAction(tr("White to black"));
+	colorMapWhiteToBlack_->setCheckable(true);
+	colorMapWhiteToBlack_->setChecked(false);
+	colorMapBlackToWhite_ = colorMap->addAction(tr("Black to white"));
+	colorMapBlackToWhite_->setCheckable(true);
+	colorMapBlackToWhite_->setChecked(false);
+	colorMapRedToBlue_ = colorMap->addAction(tr("Red to blue"));
+	colorMapRedToBlue_->setCheckable(true);
+	colorMapRedToBlue_->setChecked(true);
+	colorMapBlueToRed_ = colorMap->addAction(tr("Blue to red"));
+	colorMapBlueToRed_->setCheckable(true);
+	colorMapBlueToRed_->setChecked(false);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(colorMapWhiteToBlack_);
+	group->addAction(colorMapBlackToWhite_);
+	group->addAction(colorMapRedToBlue_);
+	group->addAction(colorMapBlueToRed_);
+	resetChanges_ = menu_->addAction(tr("Reset Changes"));
+}
+
+void EditDepthArea::setImage(const cv::Mat &depth, const cv::Mat & rgb)
+{
+	UASSERT(!depth.empty());
+	UASSERT(depth.type() == CV_32FC1 ||
+			depth.type() == CV_16UC1);
+	originalImage_ = depth;
+
+	uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
+	if(colorMapBlackToWhite_->isChecked())
+	{
+		colorMap = uCvQtDepthBlackToWhite;
+	}
+	else if(colorMapRedToBlue_->isChecked())
+	{
+		colorMap = uCvQtDepthRedToBlue;
+	}
+	else if(colorMapBlueToRed_->isChecked())
+	{
+		colorMap = uCvQtDepthBlueToRed;
+	}
+
+	image_ = uCvMat2QImage(depth, true, colorMap).convertToFormat(QImage::Format_RGB32);
+
+	imageRGB_ = QImage();
+	if(!rgb.empty())
+	{
+		imageRGB_ = uCvMat2QImage(rgb);
+		if( depth.cols != rgb.cols ||
+			depth.rows != rgb.rows)
+		{
+			// scale rgb to depth
+			imageRGB_ = imageRGB_.scaled(image_.size());
+		}
+	}
+	showRGB_->setEnabled(!imageRGB_.isNull());
+	modified_ = false;
+	update();
+}
+
+cv::Mat EditDepthArea::getModifiedImage() const
+{
+	cv::Mat modifiedImage = originalImage_.clone();
+	if(modified_)
+	{
+		UASSERT(image_.width() == modifiedImage.cols &&
+				image_.height() == modifiedImage.rows);
+		UASSERT(modifiedImage.type() == CV_32FC1 ||
+				modifiedImage.type() == CV_16UC1);
+		for(int j=0; j<image_.height(); ++j)
+		{
+			for(int i=0; i<image_.width(); ++i)
+			{
+				if(qRed(image_.pixel(i, j)) == 0 &&
+				   qGreen(image_.pixel(i, j)) == 0 &&
+				   qBlue(image_.pixel(i, j)) == 0)
+				{
+					if(modifiedImage.type() == CV_32FC1)
+					{
+						modifiedImage.at<float>(j,i) = 0.0f;
+					}
+					else // CV_16UC1
+					{
+						modifiedImage.at<unsigned short>(j,i) = 0;
+					}
+				}
+			}
+		}
+	}
+	return modifiedImage;
+}
+
+void EditDepthArea::setPenWidth(int newWidth)
+{
+    myPenWidth_ = newWidth;
+}
+
+void EditDepthArea::resetChanges()
+{
+    image_ = uCvMat2QImage(originalImage_).convertToFormat(QImage::Format_RGB32);
+    modified_ = false;
+    update();
+}
+
+void EditDepthArea::setColorMap(uCvQtDepthColorMap type)
+{
+	if(type == uCvQtDepthBlackToWhite)
+	{
+		colorMapBlackToWhite_->setChecked(true);
+	}
+	else if(type == uCvQtDepthRedToBlue)
+	{
+		colorMapRedToBlue_->setChecked(true);
+	}
+	else if(type == uCvQtDepthBlueToRed)
+	{
+		colorMapBlueToRed_->setChecked(true);
+	}
+	else
+	{
+		colorMapWhiteToBlack_->setChecked(true);
+	}
+
+	if(!originalImage_.empty())
+	{
+		image_ = uCvMat2QImage(originalImage_, true, type).convertToFormat(QImage::Format_RGB32);
+		update();
+	}
+}
+
+void EditDepthArea::mousePressEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+        lastPoint_.setX((event->pos().x()-offsetX)/scale);
+        lastPoint_.setY((event->pos().y()-offsetY)/scale);
+
+        scribbling_ = true;
+    }
+}
+
+void EditDepthArea::mouseMoveEvent(QMouseEvent *event)
+{
+    if ((event->buttons() & Qt::LeftButton) && scribbling_)
+    {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+        drawLineTo(to);
+    }
+}
+
+void EditDepthArea::mouseReleaseEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton && scribbling_) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+		drawLineTo(to);
+        scribbling_ = false;
+    }
+}
+
+void EditDepthArea::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
+{
+	scale = 1.0f;
+	offsetX = 0.0f;
+	offsetY = 0.0f;
+
+	if(!image_.isNull())
+	{
+		float w = image_.width();
+		float h = image_.height();
+		float widthRatio = float(targetRect.width()) / w;
+		float heightRatio = float(targetRect.height()) / h;
+
+		//printf("w=%f, h=%f, wR=%f, hR=%f, sW=%d, sH=%d\n", w, h, widthRatio, heightRatio, this->rect().width(), this->rect().height());
+		if(widthRatio < heightRatio)
+		{
+			scale = widthRatio;
+		}
+		else
+		{
+			scale = heightRatio;
+		}
+
+		//printf("ratio=%f\n",ratio);
+
+		w *= scale;
+		h *= scale;
+
+		if(w < targetRect.width())
+		{
+			offsetX = (targetRect.width() - w)/2.0f;
+		}
+		if(h < targetRect.height())
+		{
+			offsetY = (targetRect.height() - h)/2.0f;
+		}
+		//printf("offsetX=%f, offsetY=%f\n",offsetX, offsetY);
+	}
+}
+
+void EditDepthArea::paintEvent(QPaintEvent *event)
+{
+	//Scale
+	float ratio, offsetX, offsetY;
+	this->computeScaleOffsets(event->rect(), ratio, offsetX, offsetY);
+	QPainter painter(this);
+
+	painter.translate(offsetX, offsetY);
+	painter.scale(ratio, ratio);
+
+	if(showRGB_->isChecked() && !imageRGB_.isNull())
+	{
+		painter.setOpacity(0.5);
+		painter.drawImage(QPoint(0,0), imageRGB_);
+	}
+
+	painter.drawImage(QPoint(0,0), image_);
+}
+
+void EditDepthArea::resizeEvent(QResizeEvent *event)
+{
+    QWidget::resizeEvent(event);
+}
+
+void floodfill(QRgb * bits, const cv::Mat & depthImage, int x, int y, float lastDepthValue, float error, int &iterations)
+{
+	++iterations;
+	if(x>=0 && x<depthImage.cols &&
+	   y>=0 && y<depthImage.rows)
+	{
+		float currentValue;
+		if(depthImage.type() == CV_32FC1)
+		{
+			currentValue = depthImage.at<float>(y, x);
+		}
+		else
+		{
+			currentValue = float(depthImage.at<unsigned short>(y, x))/1000.0f;
+		}
+		if(currentValue == 0.0f)
+		{
+			return;
+		}
+
+		QRgb & rgb = bits[x+y*depthImage.cols];
+		if(qRed(rgb) == 0 && qGreen(rgb) == 0 && qBlue(rgb) == 0)
+		{
+			return;
+		}
+		if(lastDepthValue>=0.0f && fabs(lastDepthValue - currentValue) > error*lastDepthValue)
+		{
+			return;
+		}
+		rgb = 0;
+
+		if(y+1<depthImage.rows)
+		{
+			QRgb & rgb = bits[x+(y+1)*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x, y+1, currentValue, error, iterations);
+			}
+		}
+		if(y-1>=0)
+		{
+			QRgb & rgb = bits[x+(y-1)*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x, y-1, currentValue, error, iterations);
+			}
+		}
+		if(x+1<depthImage.cols)
+		{
+			QRgb & rgb = bits[x+1+y*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x+1, y, currentValue, error, iterations);
+			}
+		}
+		if(x-1>=0)
+		{
+			QRgb & rgb = bits[x-1+y*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x-1, y, currentValue, error, iterations);
+			}
+		}
+	}
+}
+
+void EditDepthArea::contextMenuEvent(QContextMenuEvent * e)
+{
+	QAction * action = menu_->exec(e->globalPos());
+	if(action == showRGB_)
+	{
+		this->update();
+	}
+	else if(action == removeCluster_)
+	{
+		float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint pixel;
+		pixel.setX((e->pos().x()-offsetX)/scale);
+		pixel.setY((e->pos().y()-offsetY)/scale);
+		if(pixel.x()>=0 && pixel.x() < originalImage_.cols &&
+		   pixel.y()>=0 && pixel.y() < originalImage_.rows)
+		{
+			int iterations=0;
+			floodfill((QRgb*)image_.bits(), originalImage_, pixel.x(), pixel.y(), -1.0f, clusterError_, iterations);
+		}
+		modified_=true;
+		this->update();
+	}
+	else if(action == clusterErrorCluster_)
+	{
+		bool ok;
+		double error = QInputDialog::getDouble(this, tr("Set Cluster Error"), tr("Error:"), clusterError(), 0.001, 1, 3, &ok);
+		if(ok)
+		{
+			clusterError_= error;
+		}
+		modified_=true;
+	}
+	else if(action == setPenWidth_)
+	{
+		bool ok;
+		int width = QInputDialog::getInt(this, tr("Set Pen Width"), tr("Width (pixels):"), penWidth(), 1, 999, 1, &ok);
+		if(ok)
+		{
+			myPenWidth_ = width;
+		}
+	}
+	else if(action == colorMapBlackToWhite_ ||
+			action == colorMapWhiteToBlack_ ||
+			action == colorMapRedToBlue_ ||
+			action == colorMapBlueToRed_)
+	{
+		uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
+		if(colorMapBlackToWhite_->isChecked())
+		{
+			colorMap = uCvQtDepthBlackToWhite;
+		}
+		else if(colorMapRedToBlue_->isChecked())
+		{
+			colorMap = uCvQtDepthRedToBlue;
+		}
+		else if(colorMapBlueToRed_->isChecked())
+		{
+			colorMap = uCvQtDepthBlueToRed;
+		}
+		this->setColorMap(colorMap);
+	}
+	else if(action == resetChanges_)
+	{
+		this->resetChanges();
+	}
+}
+
+void EditDepthArea::drawLineTo(const QPoint &endPoint)
+{
+    QPainter painter(&image_);
+    painter.setPen(QPen(Qt::black, myPenWidth_, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+    painter.drawLine(lastPoint_, endPoint);
+    modified_ = true;
+
+    update();
+    lastPoint_ = endPoint;
+}
+
+}

rtabmap/guilib/src/EditMapArea.cpp

@@ -0,0 +1,249 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <QWidget>
+#include <QPainter>
+#include <QMouseEvent>
+#include <QMenu>
+#include <QAction>
+#include <QActionGroup>
+#include <QInputDialog>
+
+#include "rtabmap/gui/EditMapArea.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+EditMapArea::EditMapArea(QWidget *parent)
+    : QWidget(parent)
+{
+    setAttribute(Qt::WA_StaticContents);
+    modified_ = false;
+    scribbling_ = false;
+    myPenWidth_ = 3;
+
+    menu_ = new QMenu(tr(""), this);
+	setPenWidth_ = menu_->addAction(tr("Set Pen Width..."));
+	addObstacle_ = menu_->addAction(tr("Add Obstacle"));
+	addObstacle_->setCheckable(true);
+	addObstacle_->setChecked(true);
+	clearObstacle_ = menu_->addAction(tr("Clear Obstacle"));
+	clearObstacle_->setCheckable(true);
+	clearObstacle_->setChecked(false);
+	setUnknown_ = menu_->addAction(tr("Set Unknown"));
+	setUnknown_->setCheckable(true);
+	setUnknown_->setChecked(false);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(addObstacle_);
+	group->addAction(clearObstacle_);
+	group->addAction(setUnknown_);
+	resetChanges_ = menu_->addAction(tr("Reset Changes"));
+}
+
+void EditMapArea::setMap(const cv::Mat &map)
+{
+	UASSERT(!map.empty());
+	UASSERT(map.type() == CV_8UC1);
+	originalMap_ = map;
+
+	map_ = uCvMat2QImage(map, true).convertToFormat(QImage::Format_RGB32);
+
+	modified_ = false;
+	update();
+}
+
+cv::Mat EditMapArea::getModifiedMap() const
+{
+	cv::Mat modifiedMap = originalMap_.clone();
+	if(modified_)
+	{
+		UASSERT(map_.width() == modifiedMap.cols &&
+				map_.height() == modifiedMap.rows);
+		UASSERT(modifiedMap.type() == CV_8UC1);
+		for(int j=0; j<map_.height(); ++j)
+		{
+			for(int i=0; i<map_.width(); ++i)
+			{
+				modifiedMap.at<unsigned char>(j,i) = qRed(map_.pixel(i, j));
+			}
+		}
+	}
+	return modifiedMap;
+}
+
+void EditMapArea::setPenWidth(int newWidth)
+{
+    myPenWidth_ = newWidth;
+}
+
+void EditMapArea::resetChanges()
+{
+    map_ = uCvMat2QImage(originalMap_).convertToFormat(QImage::Format_RGB32);
+    modified_ = false;
+    update();
+}
+
+void EditMapArea::mousePressEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+        lastPoint_.setX((event->pos().x()-offsetX)/scale);
+        lastPoint_.setY((event->pos().y()-offsetY)/scale);
+
+        scribbling_ = true;
+    }
+}
+
+void EditMapArea::mouseMoveEvent(QMouseEvent *event)
+{
+    if ((event->buttons() & Qt::LeftButton) && scribbling_)
+    {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+        drawLineTo(to);
+    }
+}
+
+void EditMapArea::mouseReleaseEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton && scribbling_) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+		drawLineTo(to);
+        scribbling_ = false;
+    }
+}
+
+void EditMapArea::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
+{
+	scale = 1.0f;
+	offsetX = 0.0f;
+	offsetY = 0.0f;
+
+	if(!map_.isNull())
+	{
+		float w = map_.width();
+		float h = map_.height();
+		float widthRatio = float(targetRect.width()) / w;
+		float heightRatio = float(targetRect.height()) / h;
+
+		//printf("w=%f, h=%f, wR=%f, hR=%f, sW=%d, sH=%d\n", w, h, widthRatio, heightRatio, this->rect().width(), this->rect().height());
+		if(widthRatio < heightRatio)
+		{
+			scale = widthRatio;
+		}
+		else
+		{
+			scale = heightRatio;
+		}
+
+		//printf("ratio=%f\n",ratio);
+
+		w *= scale;
+		h *= scale;
+
+		if(w < targetRect.width())
+		{
+			offsetX = (targetRect.width() - w)/2.0f;
+		}
+		if(h < targetRect.height())
+		{
+			offsetY = (targetRect.height() - h)/2.0f;
+		}
+		//printf("offsetX=%f, offsetY=%f\n",offsetX, offsetY);
+	}
+}
+
+void EditMapArea::paintEvent(QPaintEvent *event)
+{
+	//Scale
+	float ratio, offsetX, offsetY;
+	this->computeScaleOffsets(event->rect(), ratio, offsetX, offsetY);
+	QPainter painter(this);
+
+	painter.translate(offsetX, offsetY);
+	painter.scale(ratio, ratio);
+	painter.drawImage(QPoint(0,0), map_);
+}
+
+void EditMapArea::resizeEvent(QResizeEvent *event)
+{
+    QWidget::resizeEvent(event);
+}
+
+void EditMapArea::contextMenuEvent(QContextMenuEvent * e)
+{
+	QAction * action = menu_->exec(e->globalPos());
+	if(action == setPenWidth_)
+	{
+		bool ok;
+		int width = QInputDialog::getInt(this, tr("Set Pen Width"), tr("Width:"), penWidth(), 1, 99, 1, &ok);
+		if(ok)
+		{
+			myPenWidth_ = width;
+		}
+	}
+	else if(action == resetChanges_)
+	{
+		this->resetChanges();
+	}
+}
+
+void EditMapArea::drawLineTo(const QPoint &endPoint)
+{
+    QPainter painter(&map_);
+    QColor color;
+
+    //base on util3d::convertMap2Image8U();
+    if(addObstacle_->isChecked())
+    {
+    	color.setRgb(0,0,0);
+    }
+    else if(clearObstacle_->isChecked())
+    {
+    	color.setRgb(178,178,178);
+    }
+    else //unknown
+    {
+    	color.setRgb(89,89,89);
+    }
+    painter.setPen(QPen(color, myPenWidth_, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+    painter.drawLine(lastPoint_, endPoint);
+    modified_ = true;
+
+    update();
+    lastPoint_ = endPoint;
+}
+
+}

rtabmap/guilib/src/ExportBundlerDialog.cpp

@@ -0,0 +1,578 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ExportBundlerDialog.h"
+#include "ui_exportBundlerDialog.h"
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <QFileDialog>
+#include <QPushButton>
+#include <QMessageBox>
+#include <QTextStream>
+
+namespace rtabmap {
+
+ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_ExportBundlerDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
+
+	restoreDefaults();
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->groupBox_export_points, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
+	connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->groupBox_export_points->setEnabled(false);
+		_ui->groupBox_export_points->setChecked(false);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+
+	_ui->lineEdit_path->setText(QDir::currentPath());
+
+	updateVisibility();
+}
+
+ExportBundlerDialog::~ExportBundlerDialog()
+{
+	delete _ui;
+}
+
+void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
+	settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
+	settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
+	settings.setValue("exportPoints", _ui->groupBox_export_points->isChecked());
+	settings.setValue("sba_iterations", _ui->sba_iterations->value());
+	settings.setValue("sba_type", _ui->comboBox_sbaType->currentIndex());
+	settings.setValue("sba_variance", _ui->sba_variance->value());
+	settings.setValue("sba_rematch_features", _ui->sba_rematchFeatures->isChecked());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
+	_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
+	_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value()).toDouble());
+	_ui->groupBox_export_points->setChecked(settings.value("exportPoints", _ui->groupBox_export_points->isChecked()).toBool());
+	_ui->sba_iterations->setValue(settings.value("sba_iterations", _ui->sba_iterations->value()).toInt());
+	_ui->comboBox_sbaType->setCurrentIndex((Optimizer::Type)settings.value("sba_type", _ui->comboBox_sbaType->currentIndex()).toInt());
+	_ui->sba_variance->setValue(settings.value("sba_variance", _ui->sba_variance->value()).toDouble());
+	_ui->sba_rematchFeatures->setChecked(settings.value("sba_rematch_features", _ui->sba_rematchFeatures->isChecked()).toBool());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportBundlerDialog::setWorkingDirectory(const QString & path)
+{
+	_ui->lineEdit_path->setText((path.isEmpty()?QDir::currentPath():path) + "/bundler");
+}
+
+void ExportBundlerDialog::restoreDefaults()
+{
+	_ui->doubleSpinBox_linearSpeed->setValue(0);
+	_ui->doubleSpinBox_angularSpeed->setValue(0);
+	_ui->doubleSpinBox_laplacianVariance->setValue(0);
+	_ui->groupBox_export_points->setChecked(false);
+	_ui->sba_iterations->setValue(20);
+	if(Optimizer::isAvailable(Optimizer::kTypeG2O) || !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+
+	_ui->sba_variance->setValue(1.0);
+	_ui->sba_rematchFeatures->setChecked(true);
+}
+
+void ExportBundlerDialog::updateVisibility()
+{
+	_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+	_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+}
+
+void ExportBundlerDialog::getPath()
+{
+	QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _ui->lineEdit_path->text());
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_path->setText(path);
+	}
+}
+
+void ExportBundlerDialog::exportBundler(
+		std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const QMap<int, Signature> & signatures,
+		const ParametersMap & parameters)
+{
+	if(this->exec() != QDialog::Accepted)
+	{
+		return;
+	}
+	QString path = _ui->lineEdit_path->text();
+	if(!path.isEmpty())
+	{
+		if(!QDir(path).mkpath("."))
+		{
+			QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
+			return;
+		}
+
+		std::map<int, cv::Point3f> points3DMap;
+		std::map<int, std::map<int, FeatureBA> > wordReferences;
+		if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
+		{
+			std::map<int, Transform> posesOut;
+			std::multimap<int, Link> linksOut;
+			Optimizer::Type sbaType = _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
+			UASSERT(Optimizer::isAvailable(sbaType));
+			ParametersMap parametersSBA = parameters;
+			uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
+			uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
+			std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
+			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
+			// set input poses as initial optimization guess
+			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+			{
+				iter->second = poses.at(iter->first);
+			}
+			if(_ui->sba_iterations->value() > 0)
+			{
+				UINFO("Do BA optimization with %d iterations.", _ui->sba_iterations->value());
+				poses = sba->optimizeBA(
+						posesOut.begin()->first,
+						posesOut,
+						linksOut,
+						signatures.toStdMap(),
+						points3DMap,
+						wordReferences,
+						_ui->sba_rematchFeatures->isChecked(),
+						parametersSBA);
+			}
+			else
+			{
+				UINFO("Do not optimize, just compute 3D features and word correspondences.");
+				poses = posesOut;
+				sba->computeBACorrespondences(poses,
+					linksOut, 
+					signatures.toStdMap(),
+					points3DMap,
+					wordReferences,
+					_ui->sba_rematchFeatures->isChecked(),
+					false,
+					parametersSBA);
+			}
+
+			if(poses.empty())
+			{
+				QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
+				return;
+			}
+		}
+
+		// export cameras and images
+		QFile fileOut(path+QDir::separator()+"cameras.out");
+		QFile fileList(path+QDir::separator()+"list.txt");
+		QFile fileListKeys(path+QDir::separator()+"list_keys.txt");
+		QDir(path).mkdir("images");
+		if(wordReferences.size())
+		{
+			QDir(path).mkdir("keys");
+		}
+		if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
+		{
+			if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
+			{
+				std::map<int, Transform> cameras;
+				std::map<int, int> cameraIndexes;
+				int camIndex = 0;
+				std::map<int, QColor> colors;
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					if(ster!= signatures.end())
+					{
+						cv::Mat image = ster.value().sensorData().imageRaw();
+						if(image.empty())
+						{
+							ster.value().sensorData().uncompressDataConst(&image, 0, 0, 0);
+						}
+
+						double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
+						double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
+						double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
+						bool blurryImage = false;
+						const std::vector<float> & velocity = ster.value().getVelocity();
+						if(maxLinearVel>0.0 || maxAngularVel>0.0)
+						{
+							if(velocity.size() == 6)
+							{
+								float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
+								float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
+								if(maxLinearVel>0.0 && transVel > maxLinearVel)
+								{
+									UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
+									blurryImage = true;
+								}
+								else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
+								{
+									UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
+									blurryImage = true;
+								}
+							}
+							else
+							{
+								UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
+							}
+						}
+
+						if(!blurryImage && !image.empty() && laplacianThr>0.0)
+						{
+							cv::Mat imgLaplacian;
+							cv::Laplacian(image, imgLaplacian, CV_16S);
+							cv::Mat m, s;
+							cv::meanStdDev(imgLaplacian, m, s);
+							double stddev_pxl = s.at<double>(0);
+							double var = stddev_pxl*stddev_pxl;
+							if(var < laplacianThr)
+							{
+								blurryImage = true;
+								UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
+							}
+						}
+						if(!blurryImage)
+						{
+							cameras.insert(*iter);
+							cameraIndexes.insert(std::make_pair(iter->first, camIndex++));
+							QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
+							p = path+QDir::separator()+p;
+							if(cv::imwrite(p.toStdString(), image))
+							{
+								UINFO("saved image %s", p.toStdString().c_str());
+							}
+							else
+							{
+								UERROR("Failed to save image %s", p.toStdString().c_str());
+							}
+
+							//
+							// Descriptors
+							//
+							// The file format starts with 2 integers giving the total number of
+							// keypoints and the length of the descriptor vector for each keypoint
+							// (128). Then the location of each keypoint in the image is specified by
+							// 4 floating point numbers giving subpixel row and column location,
+							// scale, and orientation (in radians from -PI to PI).  Obviously, these
+							// numbers are not invariant to viewpoint, but can be used in later
+							// stages of processing to check for geometric consistency among matches.
+							// Finally, the invariant descriptor vector for the keypoint is given as
+							// a list of 128 integers in range [0,255].  Keypoints from a new image
+							// can be matched to those from previous images by simply looking for the
+							// descriptor vector with closest Euclidean distance among all vectors
+							// from previous images.
+							//
+							if(wordReferences.size())
+							{
+								std::list<FeatureBA> descriptors;
+								for(std::map<int, std::map<int, FeatureBA> >::iterator jter=wordReferences.begin(); jter!=wordReferences.end(); ++jter)
+								{
+									for(std::map<int, FeatureBA>::iterator kter=jter->second.begin(); kter!=jter->second.end(); ++kter)
+									{
+										if(kter->first == iter->first)
+										{
+											if(!kter->second.descriptor.empty())
+											{
+												descriptors.push_back(kter->second);
+											}
+
+											if(colors.find(jter->first) == colors.end())
+											{
+												if(!image.empty() &&
+												   kter->second.kpt.pt.x >= 0.0f && (int)kter->second.kpt.pt.x < image.cols &&
+												   kter->second.kpt.pt.y >= 0.0f && (int)kter->second.kpt.pt.y < image.rows)
+												{
+													UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC1);
+													QColor c;
+													if(image.channels() == 3)
+													{
+														cv::Vec3b & pixel = image.at<cv::Vec3b>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
+														c.setRgb(pixel[2], pixel[1], pixel[0]);
+													}
+													else // grayscale
+													{
+														unsigned char & pixel = image.at<unsigned char>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
+														c.setRgb(pixel, pixel, pixel);
+													}
+													colors.insert(std::make_pair(jter->first, c));
+												}
+											}
+										}
+									}
+								}
+
+								QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
+								p = path+QDir::separator()+p;
+								QFile fileKey(p);
+								if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
+								{
+									if(descriptors.size())
+									{
+										QTextStream key(&fileKey);
+										key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
+										for(std::list<FeatureBA>::iterator dter=descriptors.begin(); dter!=descriptors.end(); ++dter)
+										{
+											// unpack octave value to get the scale set by SIFT (https://github.com/opencv/opencv/issues/4554)
+											int octave = dter->kpt.octave & 255;
+											octave = octave < 128 ? octave : (-128 | octave);
+											float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
+
+											key << dter->kpt.pt.x << " " << dter->kpt.pt.y << " " << scale << " " << dter->kpt.angle << "\n";
+											for(int i=0; i<dter->descriptor.cols; ++i)
+											{
+												if(dter->descriptor.type() == CV_8U)
+												{
+													key << " " << (int)dter->descriptor.at<unsigned char>(i);
+												}
+												else // assume CV_32F
+												{
+													key << " " << (int)dter->descriptor.at<float>(i);
+												}
+												if((i+1)%20 == 0 && i+1 < dter->descriptor.cols)
+												{
+													key << "\n";
+												}
+											}
+											key << "\n";
+										}
+									}
+									else
+									{
+										UWARN("No descriptors saved for frame %d in file %s. "
+												"Descriptors may not have been saved in the nodes. "
+												"Verify that parameter %s was true during mapping.",
+												iter->first, p.toStdString().c_str(),
+												Parameters::kMemRawDescriptorsKept().c_str());
+									}
+									fileKey.close();
+								}
+							}
+						}
+					}
+					else
+					{
+						UWARN("Could not find node data for pose %d", iter->first);
+					}
+				}
+
+				static const Transform opengl_world_T_rtabmap_world(
+						 0.0f, -1.0f, 0.0f, 0.0f,
+						 0.0f,  0.0f, 1.0f, 0.0f,
+						-1.0f,  0.0f, 0.0f, 0.0f);
+
+				static const Transform optical_rotation_inv(
+						 0.0f, -1.0f,  0.0f, 0.0f,
+						 0.0f,  0.0f, -1.0f, 0.0f,
+						 1.0f,  0.0f,  0.0f, 0.0f);
+
+				QTextStream out(&fileOut);
+				QTextStream list(&fileList);
+				out << "# Bundle file v0.3\n";
+				out << cameras.size() << " " << points3DMap.size() << "\n";
+
+				//
+				//  Each camera entry <cameraI> contains the estimated camera intrinsics and extrinsics, and has the form:
+				//
+				//	<f> <k1> <k2>   [the focal length, followed by two radial distortion coeffs]
+				//	<R>             [a 3x3 matrix representing the camera rotation]
+				//	<t>             [a 3-vector describing the camera translation]
+				//
+				//  The cameras are specified in the order they appear in the list of images.
+				//
+				for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
+				{
+					QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
+					list << p << "\n";
+
+					Transform localTransform;
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					UASSERT(ster!=signatures.end());
+					if(ster.value().sensorData().cameraModels().size())
+					{
+						out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
+						localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
+					}
+					else if(ster.value().sensorData().stereoCameraModels().size())
+					{
+						out << ster.value().sensorData().stereoCameraModels()[0].left().fx() << " 0 0\n";
+						localTransform = ster.value().sensorData().stereoCameraModels()[0].left().localTransform();
+					}
+
+					Transform pose = iter->second;
+					if(!localTransform.isNull())
+					{
+						pose*=localTransform*optical_rotation_inv;
+					}
+					Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
+
+					out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
+					out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
+					out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
+					out << poseGL.x()   << " " << poseGL.y()   << " " << poseGL.z()   << "\n";
+				}
+				if(wordReferences.size())
+				{
+					if(fileListKeys.open(QIODevice::WriteOnly | QIODevice::Text))
+					{
+						QTextStream listKeys(&fileListKeys);
+						for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
+						{
+							QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
+							listKeys << p << "\n";
+						}
+						fileListKeys.close();
+					}
+				}
+
+				//
+				//  Each point entry has the form:
+				//
+				//		<position>      [a 3-vector describing the 3D position of the point]
+				//		<color>         [a 3-vector describing the RGB color of the point]
+				//		<view list>     [a list of views the point is visible in]
+				//
+				//	The view list begins with the length of the list (i.e., the number of cameras
+				//	the point is visible in). The list is then given as a list of quadruplets
+				//	<camera> <key> <x> <y>, where <camera> is a camera index, <key> the index
+				//	of the SIFT keypoint where the point was detected in that camera, and <x>
+				//	and <y> are the detected positions of that keypoint. Both indices are
+				//	0-based (e.g., if camera 0 appears in the list, this corresponds to the
+				//	first camera in the scene file and the first image in "list.txt"). The
+				//	pixel positions are floating point numbers in a coordinate system where
+				//	the origin is the center of the image, the x-axis increases to the right,
+				//	and the y-axis increases towards the top of the image. Thus, (-w/2, -h/2)
+				//	is the lower-left corner of the image, and (w/2, h/2) is the top-right
+				//	corner (where w and h are the width and height of the image).
+				//
+				std::map<int, int> descriptorIndexes;
+				for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+				{
+					std::map<int, std::map<int, FeatureBA> >::iterator jter = wordReferences.find(iter->first);
+					out << iter->second.x << " " << iter->second.y << " " << iter->second.z << "\n";
+					UASSERT(colors.find(iter->first) != colors.end());
+					QColor & c = colors.at(iter->first);
+					out << c.red() << " " << c.green() << " " << c.blue() << "\n";
+					out << jter->second.size();
+					for(std::map<int, FeatureBA>::iterator kter = jter->second.begin(); kter!=jter->second.end(); ++kter)
+					{
+						// <camera> <key> <x> <y>
+						int camId = kter->first;
+						UASSERT(signatures.contains(camId));
+						UASSERT(cameraIndexes.find(camId) != cameraIndexes.end());
+						const Signature & s = signatures[camId];
+						cv::Point2f pt;
+						if(signatures[camId].sensorData().cameraModels().size())
+						{
+							pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
+							pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
+						}
+						else if(signatures[camId].sensorData().stereoCameraModels().size())
+						{
+							pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModels()[0].left().cx();
+							pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModels()[0].left().cy();
+						}
+						descriptorIndexes.insert(std::make_pair(camId, 0));
+						out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;
+					}
+					out << "\n";
+				}
+
+				fileList.close();
+				fileOut.close();
+
+				QMessageBox::information(this,
+						tr("Exporting cameras in Bundler format..."),
+						tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
+						.arg(poses.size())
+						.arg(points3DMap.size())
+						.arg(path)
+						.arg(poses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(poses.size()-cameras.size()).arg(poses.size()):""));
+			}
+			else
+			{
+				fileOut.close();
+				QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
+			}
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
+		}
+	}
+}
+
+}

rtabmap/guilib/src/ExportDialog.cpp

@@ -0,0 +1,170 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ExportDialog.h"
+#include "ui_exportDialog.h"
+
+#include <QFileDialog>
+#include <QPushButton>
+
+namespace rtabmap {
+
+ExportDialog::ExportDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_ExportDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
+
+	restoreDefaults();
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_framerate, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_session, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	_ui->lineEdit_path->setText(QDir::currentPath()+QDir::separator()+"output.db");
+}
+
+ExportDialog::~ExportDialog()
+{
+	delete _ui;
+}
+
+void ExportDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("framesIgnored", this->framesIgnored());
+	settings.setValue("targetFramerate", this->targetFramerate());
+	settings.setValue("sessionExported", this->sessionExported());
+	settings.setValue("rgbExported", this->isRgbExported());
+	settings.setValue("depthExported", this->isDepthExported());
+	settings.setValue("depth2dExported", this->isDepth2dExported());
+	settings.setValue("odomExported", this->isOdomExported());
+	settings.setValue("userDataExported", this->isUserDataExported());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	_ui->spinBox_ignored->setValue(settings.value("framesIgnored", this->framesIgnored()).toInt());
+	_ui->doubleSpinBox_framerate->setValue(settings.value("targetFramerate", this->targetFramerate()).toDouble());
+	_ui->spinBox_session->setValue(settings.value("sessionExported", this->sessionExported()).toInt());
+	_ui->checkBox_rgb->setChecked(settings.value("rgbExported", this->isRgbExported()).toBool());
+	_ui->checkBox_depth->setChecked(settings.value("depthExported", this->isDepthExported()).toBool());
+	_ui->checkBox_depth2d->setChecked(settings.value("depth2dExported", this->isDepth2dExported()).toBool());
+	_ui->checkBox_odom->setChecked(settings.value("odomExported", this->isOdomExported()).toBool());
+	_ui->checkBox_userData->setChecked(settings.value("userDataExported", this->isUserDataExported()).toBool());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::restoreDefaults()
+{
+	_ui->spinBox_ignored->setValue(0);
+	_ui->doubleSpinBox_framerate->setValue(0);
+	_ui->spinBox_session->setValue(-1);
+	_ui->checkBox_rgb->setChecked(true);
+	_ui->checkBox_depth->setChecked(true);
+	_ui->checkBox_depth2d->setChecked(true);
+	_ui->checkBox_odom->setChecked(true);
+	_ui->checkBox_userData->setChecked(false);
+}
+
+void ExportDialog::getPath()
+{
+	QString path = QFileDialog::getSaveFileName(this, tr("Output database path..."), _ui->lineEdit_path->text(), tr("RTAB-Map database (*.db)"));
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_path->setText(path);
+	}
+}
+
+QString ExportDialog::outputPath() const
+{
+	return _ui->lineEdit_path->text();
+}
+
+int ExportDialog::framesIgnored() const
+{
+	return _ui->spinBox_ignored->value();
+}
+
+double ExportDialog::targetFramerate() const
+{
+	return _ui->doubleSpinBox_framerate->value();
+}
+
+int ExportDialog::sessionExported() const
+{
+	return _ui->spinBox_session->value();
+}
+
+bool ExportDialog::isRgbExported() const
+{
+	return _ui->checkBox_rgb->isChecked();
+}
+
+bool ExportDialog::isDepthExported() const
+{
+	return _ui->checkBox_depth->isChecked();
+}
+
+bool ExportDialog::isDepth2dExported() const
+{
+	return _ui->checkBox_depth2d->isChecked();
+}
+
+bool ExportDialog::isOdomExported() const
+{
+	return _ui->checkBox_odom->isChecked();
+}
+
+bool ExportDialog::isUserDataExported() const
+{
+	return _ui->checkBox_userData->isChecked();
+}
+
+}

rtabmap/guilib/src/GuiLib.qrc

@@ -0,0 +1,50 @@
+<RCC>
+    <qresource prefix="/">
+        <file>images/Stop1NormalYellow.png</file>
+        <file>images/PauseNormalRed.png</file>
+        <file>images/PauseNormal.png</file>
+        <file>images/Play1Normal.png</file>
+        <file>images/Pause.ico</file>
+        <file>images/PauseOnLoop.ico</file>
+        <file>images/PauseOnLocalLoop.ico</file>
+        <file>images/PauseLoopRejected.ico</file>
+        <file>qss/default.qss</file>
+        <file>images/Plot16.png</file>
+        <file>images/Plot48.png</file>
+        <file>images/RTAB-Map.ico</file>
+        <file>images/RTAB-Map.png</file>
+        <file>images/IntRoLab.png</file>
+        <file>images/IntRoLabSmall.png</file>
+        <file>images/metal_7280826_512.jpg</file>
+        <file>images/crosshatch_metal_grille_9280154_150.JPG</file>
+        <file>images/mag_glass.png</file>
+        <file>images/document-open.png</file>
+        <file>images/document-new.png</file>
+        <file>images/document-save.png</file>
+        <file>images/document-properties.png</file>
+        <file>images/view-refresh.png</file>
+        <file>images/system-log-out.png</file>
+        <file>images/kinect_xbox_360.png</file>
+        <file>images/kinect_xbox_one.png</file>
+        <file>images/k4a.png</file>
+        <file>images/sense.png</file>
+        <file>images/xtion_pro_live.png</file>
+        <file>images/bumblebee2.png</file>
+        <file>images/webcam.png</file>
+        <file>images/zed.png</file>
+        <file>images/r200.png</file>
+        <file>images/zr300.png</file>
+        <file>images/d435.png</file>
+        <file>images/d415.png</file>
+        <file>images/tara.png</file>
+        <file>images/t265.png</file>
+        <file>images/sr300.png</file>
+        <file>images/mynteyes.png</file>
+        <file>images/l515.png</file>
+        <file>images/oakd.png</file>
+        <file>images/oakd_lite.png</file>
+        <file>images/astra.png</file>
+        <file>images/oakdpro.png</file>
+        <file>images/seer_sense_DS80.png</file>
+    </qresource>
+</RCC>

rtabmap/guilib/src/KeypointItem.cpp

@@ -0,0 +1,163 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/KeypointItem.h"
+
+#include <QtGui/QPen>
+#include <QtGui/QBrush>
+#include <QGraphicsScene>
+#include "rtabmap/utilite/ULogger.h"
+
+namespace rtabmap {
+
+KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, float depth, const QColor & color, QGraphicsItem * parent) :
+	QGraphicsEllipseItem(kpt.pt.x-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.pt.y-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.size==0?3.0f:kpt.size, kpt.size==0?3.0f:kpt.size, parent),
+	_id(id),
+	_kpt(kpt),
+	_placeHolder(0),
+	_depth(depth)
+{
+	this->setColor(color);
+	this->setAcceptHoverEvents(true);
+	this->setFlag(QGraphicsItem::ItemIsFocusable, true);
+	_width = pen().width();
+}
+
+KeypointItem::~KeypointItem()
+{
+	delete _placeHolder;
+}
+
+void KeypointItem::setColor(const QColor & color)
+{
+	this->setPen(QPen(color));
+	this->setBrush(QBrush(color));
+}
+
+void KeypointItem::showDescription()
+{
+	if(!_placeHolder)
+	{
+		_placeHolder = new QGraphicsRectItem (this);
+		_placeHolder->setVisible(false);
+		if(qGray(pen().color().rgb()) > 255/2)
+		{
+			_placeHolder->setBrush(QBrush(QColor ( 0,0,0, 170 )));
+		}
+		else
+		{
+			_placeHolder->setBrush(QBrush(QColor ( 255, 255, 255, 170 )));
+		}
+		QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
+		text->setDefaultTextColor(this->pen().color().rgb());
+		// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
+		int octave = _kpt.octave & 255;
+		octave = octave < 128 ? octave : (-128 | octave);
+		float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
+		if(_depth <= 0)
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Octave = %8\n"
+					"Scale = %9").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale));
+		}
+		else
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Octave = %8\n"
+					"Scale = %9\n"
+					"Depth = %10 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale).arg(_depth));
+		}
+		_placeHolder->setRect(text->boundingRect());
+	}
+
+
+	if(_placeHolder->parentItem())
+	{
+		_placeHolder->setParentItem(0); // Make it a to level item
+	}
+	QPen pen = this->pen();
+	this->setPen(QPen(pen.color(), _width+2));
+	_placeHolder->setZValue(this->zValue()+1);
+	_placeHolder->setPos(this->mapFromScene(0,0));
+	_placeHolder->setVisible(true);
+}
+
+void KeypointItem::hideDescription()
+{
+	if(_placeHolder)
+	{
+		_placeHolder->setVisible(false);
+	}
+	this->setPen(QPen(pen().color(), _width));
+}
+
+void KeypointItem::hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+{
+	QGraphicsScene * scene = this->scene();
+	if(scene && scene->focusItem() == 0)
+	{
+		this->showDescription();
+	}
+	else
+	{
+		this->setPen(QPen(pen().color(), _width+2));
+	}
+	QGraphicsEllipseItem::hoverEnterEvent(event);
+}
+
+void KeypointItem::hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
+{
+	if(!this->hasFocus())
+	{
+		this->hideDescription();
+	}
+	QGraphicsEllipseItem::hoverEnterEvent(event);
+}
+
+void KeypointItem::focusInEvent ( QFocusEvent * event )
+{
+	this->showDescription();
+	QGraphicsEllipseItem::focusInEvent(event);
+}
+
+void KeypointItem::focusOutEvent ( QFocusEvent * event )
+{
+	this->hideDescription();
+	QGraphicsEllipseItem::focusOutEvent(event);
+}
+
+}

rtabmap/guilib/src/LinkRefiningDialog.cpp

@@ -0,0 +1,156 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/LinkRefiningDialog.h"
+#include "ui_linkRefiningDialog.h"
+
+#include <QPushButton>
+
+namespace rtabmap {
+
+LinkRefiningDialog::LinkRefiningDialog(QWidget * parent) :
+	QDialog(parent),
+	defaultNodeIdMin_(0),
+	defaultNodeIdMax_(0),
+	defaultMapIdMin_(0),
+	defaultMapIdMax_(0)
+{
+	ui_ = new Ui_linkRefiningDialog();
+	ui_->setupUi(this);
+
+	ui_->comboBox_link_type->addItem("All");
+	for(int i =0; i<Link::kPosePrior; ++i)
+	{
+		ui_->comboBox_link_type->addItem(Link::typeName((Link::Type)i).c_str());
+		if(((Link::Type)i) == Link::kVirtualClosure)
+		{
+			ui_->comboBox_link_type->setItemData(i+1, 0, Qt::UserRole - 1);
+		}
+	}
+
+	restoreDefaults();
+
+	connect(ui_->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+	connect(ui_->comboBox_link_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateIntraInterState()));
+	connect(ui_->spinBox_node_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId()));
+	connect(ui_->spinBox_node_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId()));
+	connect(ui_->spinBox_map_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId()));
+	connect(ui_->spinBox_map_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId()));
+}
+
+LinkRefiningDialog::~LinkRefiningDialog()
+{
+	delete ui_;
+}
+
+void LinkRefiningDialog::setMinMax(
+		int nodeIdMin,
+		int nodeIdMax,
+		int mapIdMin,
+		int mapIdMax)
+{
+	bool reset = defaultNodeIdMin_ == 0;
+	defaultNodeIdMin_ = nodeIdMin;
+	defaultNodeIdMax_ = nodeIdMax;
+	defaultMapIdMin_ = mapIdMin;
+	defaultMapIdMax_ = mapIdMax;
+	ui_->spinBox_node_from->setMinimum(defaultNodeIdMin_);
+	ui_->spinBox_node_to->setMaximum(defaultNodeIdMax_);
+	ui_->spinBox_map_from->setMinimum(defaultMapIdMin_);
+	ui_->spinBox_map_to->setMaximum(defaultMapIdMax_);
+	if(reset)
+	{
+		restoreDefaults();
+	}
+}
+
+Link::Type LinkRefiningDialog::getLinkType() const
+{
+	if(ui_->comboBox_link_type->currentIndex() == 0)
+	{
+		return Link::kEnd;
+	}
+	return (Link::Type)(ui_->comboBox_link_type->currentIndex()-1);
+}
+
+void LinkRefiningDialog::getIntraInterSessions(bool & intra, bool & inter) const
+{
+	intra = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 1 || getLinkType() == Link::kNeighbor;
+	inter = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 2;
+}
+
+bool LinkRefiningDialog::isRangeByNodeId() const
+{
+	return ui_->radioButton_nodes->isChecked();
+}
+
+bool LinkRefiningDialog::isRangeByMapId() const
+{
+	return ui_->radioButton_maps->isChecked();
+}
+
+void LinkRefiningDialog::getRangeNodeId(int & from, int & to) const
+{
+	from = ui_->spinBox_node_from->value();
+	to = ui_->spinBox_node_to->value();
+}
+
+void LinkRefiningDialog::getRangeMapId(int & from, int & to) const
+{
+	from = ui_->spinBox_map_from->value();
+	to = ui_->spinBox_map_to->value();
+}
+
+void LinkRefiningDialog::restoreDefaults()
+{
+	ui_->comboBox_link_type->setCurrentIndex(0);
+	ui_->comboBox_link_inter_intra->setCurrentIndex(0);
+	ui_->spinBox_node_from->setValue(defaultNodeIdMin_);
+	ui_->spinBox_node_to->setValue(defaultNodeIdMax_);
+	ui_->spinBox_map_from->setValue(defaultMapIdMin_);
+	ui_->spinBox_map_to->setValue(defaultMapIdMax_);
+
+	ui_->radioButton_nodes->setChecked(true);
+}
+
+void LinkRefiningDialog::updateIntraInterState()
+{
+	ui_->comboBox_link_inter_intra->setEnabled(getLinkType() != Link::kNeighbor);
+}
+
+void LinkRefiningDialog::setRangeToNodeId()
+{
+	ui_->radioButton_nodes->setChecked(true);
+}
+
+void LinkRefiningDialog::setRangeToMapId()
+{
+	ui_->radioButton_maps->setChecked(true);
+}
+
+
+}

rtabmap/guilib/src/LoopClosureViewer.cpp

@@ -0,0 +1,154 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/LoopClosureViewer.h"
+#include "ui_loopClosureViewer.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
+	QWidget(parent),
+	decimation_(1),
+	maxDepth_(0),
+	minDepth_(0)
+{
+	ui_ = new Ui_loopClosureViewer();
+	ui_->setupUi(this);
+	ui_->cloudViewerTransform->setCameraLockZ(false);
+
+	connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
+}
+
+LoopClosureViewer::~LoopClosureViewer() {
+	delete ui_;
+}
+
+void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
+{
+	sA_ = sA;
+	sB_ = sB;
+	if(sA_.id()>0 && sB_.id()>0)
+	{
+		ui_->label_idA->setText(QString("[%1-%2]").arg(sA.id()).arg(sB.id()));
+	}
+}
+
+void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters)
+{
+	if(sA_.id()>0 && sB_.id()>0)
+	{
+		int decimation = 1;
+		float maxDepth = 0;
+		float minDepth = 0;
+
+		if(!ui_->checkBox_rawCloud->isChecked())
+		{
+			decimation = decimation_;
+			maxDepth = maxDepth_;
+			minDepth = minDepth_;
+		}
+
+		UDEBUG("decimation = %d", decimation);
+		UDEBUG("maxDepth = %f", maxDepth);
+		UDEBUG("minDepth = %d", minDepth);
+
+		Transform t;
+		if(!transform.isNull())
+		{
+			transform_ = transform;
+			t = transform;
+		}
+		else if(!transform_.isNull())
+		{
+			t = transform_;
+		}
+		else
+		{
+			t = sB_.getPose();
+		}
+
+		UDEBUG("t= %s", t.prettyPrint().c_str());
+		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
+		if(!t.isNull())
+		{
+			//cloud 3d
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
+			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
+			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
+
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw(), sA_.sensorData().laserScanRaw().localTransform());
+			scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw(), sB_.sensorData().laserScanRaw().localTransform());
+
+			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
+
+			if(cloudA->size())
+			{
+				ui_->cloudViewerTransform->addCloud("cloud0", cloudA);
+			}
+			if(cloudB->size())
+			{
+				cloudB = util3d::transformPointCloud(cloudB, t);
+				ui_->cloudViewerTransform->addCloud("cloud1", cloudB);
+			}
+			if(scanA->size())
+			{
+				ui_->cloudViewerTransform->addCloud("scan0", scanA);
+			}
+			if(scanB->size())
+			{
+				scanB = util3d::transformPointCloud(scanB, t);
+				ui_->cloudViewerTransform->addCloud("scan1", scanB);
+			}
+		}
+		else
+		{
+			UERROR("loop transform is null !?!?");
+			ui_->cloudViewerTransform->removeAllClouds();
+		}
+		ui_->cloudViewerTransform->refreshView();
+	}
+}
+
+
+void LoopClosureViewer::showEvent(QShowEvent * event)
+{
+	QWidget::showEvent( event );
+	QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
+}
+

rtabmap/guilib/src/MapVisibilityWidget.cpp

@@ -0,0 +1,143 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/MapVisibilityWidget.h"
+
+#include <QCheckBox>
+#include <QVBoxLayout>
+#include <QScrollArea>
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+MapVisibilityWidget::MapVisibilityWidget(QWidget * parent) : QWidget(parent) {
+
+	QVBoxLayout * verticalLayout1 = new QVBoxLayout(this);
+	QScrollArea * scrollArea = new QScrollArea(this);
+	scrollArea->setWidgetResizable(true);
+	QWidget * scrollAreaWidgetContent = new QWidget();
+	scrollAreaWidgetContent->setObjectName("area");
+	QVBoxLayout * layout2 = new QVBoxLayout(scrollAreaWidgetContent);
+	scrollAreaWidgetContent->setLayout(layout2);
+	scrollArea->setWidget(scrollAreaWidgetContent);
+
+	QCheckBox * selectAll = new QCheckBox("Select all", this);
+	connect(selectAll, SIGNAL(toggled(bool)), this, SLOT(selectAll(bool)));
+	verticalLayout1->addWidget(selectAll);
+	verticalLayout1->addWidget(scrollArea);
+}
+
+MapVisibilityWidget::~MapVisibilityWidget() {
+
+}
+
+void MapVisibilityWidget::showEvent(QShowEvent * event)
+{
+	updateCheckBoxes();
+}
+
+void MapVisibilityWidget::clear()
+{
+	_poses.clear();
+	_mask.clear();
+	updateCheckBoxes();
+}
+
+void MapVisibilityWidget::updateCheckBoxes()
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QVBoxLayout * layout = (QVBoxLayout *)area->layout();
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	while(checkboxes.size() && checkboxes.size() > (int)_poses.size())
+	{
+		delete *checkboxes.begin();
+		checkboxes.erase(checkboxes.begin());
+	}
+	int i=0;
+	for(std::map<int, Transform>::iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
+	{
+		bool added = false;
+		if(i >= checkboxes.size())
+		{
+			checkboxes.push_back(new QCheckBox(area));
+			added = true;
+		}
+		checkboxes[i]->setText(QString("%1 (%2)").arg(iter->first).arg(iter->second.prettyPrint().c_str()));
+		checkboxes[i]->setChecked(_mask.at(iter->first));
+		if(added)
+		{
+			connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(signalVisibility()));
+			layout->addWidget(checkboxes[i]);
+		}
+
+		++i;
+	}
+}
+
+void MapVisibilityWidget::setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask)
+{
+	UASSERT(poses.size() == mask.size());
+	_poses = poses;
+	_mask = mask;
+	if(this->isVisible())
+	{
+		updateCheckBoxes();
+	}
+}
+
+std::map<int, Transform> MapVisibilityWidget::getVisiblePoses() const
+{
+	std::map<int, Transform> poses;
+	for(std::map<int, Transform>::const_iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
+	{
+		if(_mask.at(iter->first) && iter->first > 0)
+		{
+			poses.insert(*iter);
+		}
+	}
+	return poses;
+}
+
+void MapVisibilityWidget::signalVisibility()
+{
+	QCheckBox * check = qobject_cast<QCheckBox*>(sender());
+	_mask.at(check->text().split('(').first().toInt()) = check->isChecked();
+	Q_EMIT visibilityChanged(check->text().split('(').first().toInt(), check->isChecked());
+}
+
+void MapVisibilityWidget::selectAll(bool checked)
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	for(int i = 0; i<checkboxes.size(); ++i)
+	{
+		checkboxes[i]->setChecked(checked);
+	}
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/MultiSessionLocSubView.cpp

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "rtabmap/gui/MultiSessionLocSubView.h"
+#include "ui_multiSessionLocSubView.h"
+
+namespace rtabmap {
+
+MultiSessionLocSubView::MultiSessionLocSubView(ImageView * mainView, int mapId, QWidget * parent) :
+		QWidget(parent),
+		mapId_(mapId)
+{
+	ui_ = new Ui_multiSessionLocSubView();
+	ui_->setupUi(this);
+	ui_->imageView->setFeaturesShown(mainView->isFeaturesShown());
+	ui_->imageView->setFeaturesSize(mainView->getFeaturesSize());
+	ui_->imageView->setAlpha(mainView->getAlpha());
+	ui_->imageView->setDefaultMatchingFeatureColor(mainView->getDefaultMatchingFeatureColor());
+}
+MultiSessionLocSubView::~MultiSessionLocSubView() {}
+
+void MultiSessionLocSubView::updateView(
+		int nodeId,
+		const QImage & image,
+		const std::multimap<int, cv::KeyPoint> & features,
+		float locRatio,
+		const QColor & bgColor)
+{
+	if(image.isNull())
+	{
+		ui_->imageView->clear();
+	}
+	else
+	{
+		ui_->imageView->setImage(image);
+		ui_->imageView->setFeatures(features, cv::Mat(), ui_->imageView->getDefaultMatchingFeatureColor());
+		ui_->imageView->setBackgroundColor(bgColor);
+	}
+	ui_->label->setText(QString("%1 [%2]").arg(nodeId).arg(mapId_));
+	ui_->locProgressBar->setValue(locRatio * 100);
+}
+
+void MultiSessionLocSubView::clear()
+{
+	ui_->imageView->clear();
+	ui_->label->setText(QString("[%1]").arg(mapId_));
+}
+
+} /* namespace rtabmap */
+

rtabmap/guilib/src/MultiSessionLocWidget.cpp

@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/MultiSessionLocWidget.h"
+#include "rtabmap/gui/MultiSessionLocSubView.h"
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/core/Graph.h"
+
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <QProgressBar>
+
+namespace rtabmap {
+
+MultiSessionLocWidget::MultiSessionLocWidget(
+		const QMap<int, Signature> * cache,
+		const std::map<int, int> * mapIds,
+		QWidget * parent) :
+				QWidget(parent),
+				cache_(cache),
+				mapIds_(mapIds),
+				totalFrames_(0),
+				totalLoops_(0)
+{
+	UASSERT(cache != 0);
+	UASSERT(mapIds != 0);
+	imageView_ = new ImageView(this);
+	imageView_->setObjectName("multisession_imageview");
+	totalLocProgressBar_ = new QProgressBar(this);
+	resetbutton_ = new QPushButton(this);
+	resetbutton_->setText("Reset");
+
+	// setup layout
+	this->setLayout(new QHBoxLayout());
+	QVBoxLayout * vLayout = new QVBoxLayout();
+	vLayout->addWidget(imageView_, 1);
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	hLayout->addWidget(resetbutton_, 0);
+	hLayout->addWidget(totalLocProgressBar_, 1);
+	vLayout->addLayout(hLayout, 0);
+	((QHBoxLayout*)this->layout())->addLayout(vLayout, 1);
+
+	connect(resetbutton_, SIGNAL(clicked()), this, SLOT(clear()));
+}
+MultiSessionLocWidget::~MultiSessionLocWidget() {}
+
+void MultiSessionLocWidget::updateView(
+		const Signature & lastSignature,
+		const Statistics & stats)
+{
+	++totalFrames_;
+	std::multimap<int, Link> loopLinks = graph::filterLinks(lastSignature.getLinks(), Link::kGlobalClosure, true);
+	std::multimap<int, Link> localLinks = graph::filterLinks(lastSignature.getLinks(), Link::kLocalSpaceClosure, true);
+	loopLinks.insert(localLinks.begin(), localLinks.end());
+
+	if(!loopLinks.empty())
+	{
+		++totalLoops_;
+		totalLocProgressBar_->setValue(float(totalLoops_)/float(totalFrames_) * 100);
+	}
+
+	if(!lastSignature.sensorData().imageRaw().empty() ||
+	   !lastSignature.sensorData().imageCompressed().empty())
+	{
+		cv::Mat image;
+		lastSignature.sensorData().uncompressDataConst(&image, 0);
+		if(!image.empty())
+		{
+			imageView_->setImage(uCvMat2QImage(image));
+			imageView_->setFeatures(lastSignature.getWordsKpts(), cv::Mat(), imageView_->getDefaultMatchingFeatureColor());
+			imageView_->setBackgroundColor(Qt::black);
+		}
+		else
+		{
+			imageView_->clear();
+		}
+	}
+	else
+	{
+		imageView_->clear();
+	}
+
+	std::map<int, std::pair<int, float> > top;
+	const std::map<int, float> & likelihood = stats.posterior();
+	for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
+	{
+		if(mapIds_->find(iter->first) != mapIds_->end())
+		{
+			int mapId = mapIds_->at(iter->first);
+			if(subViews_.find(mapId)==subViews_.end())
+			{
+				MultiSessionLocSubView * subView = new MultiSessionLocSubView(imageView_, mapId, this);
+				((QHBoxLayout*)this->layout())->addWidget(subView, 1);
+				subViews_.insert(std::make_pair(mapId, std::make_pair(subView, 0)));
+			}
+
+			if(top.find(mapId) == top.end() || top.at(mapId).second < iter->second)
+			{
+				uInsert(top, std::make_pair(mapId, std::make_pair(iter->first, iter->second)));
+			}
+		}
+	}
+
+	// update highest loop closure hypotheses per session
+	for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
+	{
+
+		for(std::multimap<int, Link>::iterator jter=loopLinks.begin(); jter!=loopLinks.end(); ++jter)
+		{
+			if(mapIds_->find(jter->first)!= mapIds_->end())
+			{
+				int mapId = mapIds_->at(jter->first);
+				iter->second.second += mapId==iter->first?1:0;
+			}
+		}
+
+		if(uContains(top, iter->first))
+		{
+			int nodeId = top.at(iter->first).first;
+			if(cache_->contains(nodeId))
+			{
+				cv::Mat image;
+				const Signature & s = (*cache_)[nodeId];
+
+				Link link = loopLinks.find(nodeId) != loopLinks.end()?loopLinks.find(nodeId)->second:Link();
+				std::multimap<int, cv::KeyPoint> keypoints;
+				for(std::multimap<int, int>::const_iterator jter=s.getWords().begin(); jter!=s.getWords().end(); ++jter)
+				{
+					if(jter->first>0 && lastSignature.getWords().find(jter->first) != lastSignature.getWords().end())
+					{
+						keypoints.insert(std::make_pair(jter->first, s.getWordsKpts()[jter->second]));
+					}
+				}
+
+				if(!keypoints.empty())
+				{
+					s.sensorData().uncompressDataConst(&image, 0);
+					if(!image.empty())
+					{
+						iter->second.first->updateView(
+								nodeId,
+								uCvMat2QImage(image),
+								keypoints,
+								float(iter->second.second)/float(totalFrames_),
+								link.type() == Link::kLocalSpaceClosure?Qt::yellow:
+								link.type() == Link::kGlobalClosure?Qt::green:Qt::gray);
+					}
+					else
+					{
+						iter->second.first->clear();
+					}
+				}
+				else
+				{
+					iter->second.first->clear();
+				}
+			}
+			else
+			{
+				iter->second.first->clear();
+			}
+		}
+		else
+		{
+			iter->second.first->clear();
+		}
+	}
+}
+void MultiSessionLocWidget::clear()
+{
+	for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
+	{
+		delete iter->second.first;
+	}
+	subViews_.clear();
+	imageView_->clear();
+	totalFrames_ = 0;
+	totalLoops_ = 0;
+	totalLocProgressBar_->setValue(0);
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/OdometryViewer.cpp

@@ -0,0 +1,515 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/OdometryViewer.h"
+
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UCv2Qt.h"
+
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/gui/CloudViewer.h"
+
+#include <QPushButton>
+#include <QSpinBox>
+#include <QDoubleSpinBox>
+#include <QCheckBox>
+#include <QLabel>
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QApplication>
+
+namespace rtabmap {
+
+OdometryViewer::OdometryViewer(
+	int maxClouds, 
+	int decimation, 
+	float voxelSize, 
+	float maxDepth, 
+	int qualityWarningThr, 
+	QWidget * parent,
+	const ParametersMap & parameters) :
+		QDialog(parent),
+		imageView_(new ImageView(this)),
+		cloudView_(new CloudViewer(this)),
+		processingData_(false),
+		odomImageShow_(true),
+		odomImageDepthShow_(true),
+		lastOdomPose_(Transform::getIdentity()),
+		qualityWarningThr_(qualityWarningThr),
+		id_(0),
+		validDecimationValue_(1),
+		parameters_(parameters)
+{
+
+	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
+
+	imageView_->setImageDepthShown(false);
+	imageView_->setMinimumSize(320, 240);
+	imageView_->setAlpha(255);
+
+	cloudView_->setCameraTargetLocked();
+	cloudView_->setGridShown(true);
+	cloudView_->setFrustumShown(true);
+
+	QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
+	QLabel * voxelLabel = new QLabel("Voxel", this);
+	QLabel * maxDepthLabel = new QLabel("Max depth", this);
+	QLabel * decimationLabel = new QLabel("Decimation", this);
+	maxCloudsSpin_ = new QSpinBox(this);
+	maxCloudsSpin_->setMinimum(0);
+	maxCloudsSpin_->setMaximum(100);
+	maxCloudsSpin_->setValue(maxClouds);
+	voxelSpin_ = new QDoubleSpinBox(this);
+	voxelSpin_->setMinimum(0);
+	voxelSpin_->setMaximum(1);
+	voxelSpin_->setDecimals(3);
+	voxelSpin_->setSingleStep(0.01);
+	voxelSpin_->setSuffix(" m");
+	voxelSpin_->setValue(voxelSize);
+	maxDepthSpin_ = new QDoubleSpinBox(this);
+	maxDepthSpin_->setMinimum(0);
+	maxDepthSpin_->setMaximum(100);
+	maxDepthSpin_->setDecimals(0);
+	maxDepthSpin_->setSingleStep(1);
+	maxDepthSpin_->setSuffix(" m");
+	maxDepthSpin_->setValue(maxDepth);
+	decimationSpin_ = new QSpinBox(this);
+	decimationSpin_->setMinimum(1);
+	decimationSpin_->setMaximum(16);
+	decimationSpin_->setValue(decimation);
+	cloudShown_ = new QCheckBox(this);
+	cloudShown_->setText("Cloud");
+	cloudShown_->setChecked(true);
+	scanShown_ = new QCheckBox(this);
+	scanShown_->setText("Scan");
+	scanShown_->setChecked(true);
+	featuresShown_ = new QCheckBox(this);
+	featuresShown_->setText("Features");
+	featuresShown_->setChecked(true);
+	timeLabel_ = new QLabel(this);
+	QPushButton * resetButton = new QPushButton("reset", this);
+	QPushButton * clearButton = new QPushButton("clear", this);
+	QPushButton * closeButton = new QPushButton("close", this);
+	connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
+	connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
+	connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
+
+	//layout
+	QHBoxLayout * layout = new QHBoxLayout();
+	layout->setContentsMargins(0,0,0,0);
+	layout->setSpacing(0);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QHBoxLayout * hlayout2 = new QHBoxLayout();
+	hlayout2->setContentsMargins(0,0,0,0);
+	hlayout2->addWidget(maxCloudsLabel);
+	hlayout2->addWidget(maxCloudsSpin_);
+	hlayout2->addWidget(voxelLabel);
+	hlayout2->addWidget(voxelSpin_);
+	hlayout2->addWidget(maxDepthLabel);
+	hlayout2->addWidget(maxDepthSpin_);
+	hlayout2->addWidget(decimationLabel);
+	hlayout2->addWidget(decimationSpin_);
+	hlayout2->addWidget(cloudShown_);
+	hlayout2->addWidget(scanShown_);
+	hlayout2->addWidget(featuresShown_);
+	hlayout2->addWidget(timeLabel_);
+	hlayout2->addStretch(1);
+	hlayout2->addWidget(resetButton);
+	hlayout2->addWidget(clearButton);
+	hlayout2->addWidget(closeButton);
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setContentsMargins(0,0,0,0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addLayout(hlayout2);
+
+	this->setLayout(vlayout);
+}
+
+OdometryViewer::~OdometryViewer()
+{
+	this->unregisterFromEventsManager();
+	this->clear();
+	UDEBUG("");
+}
+
+void OdometryViewer::reset()
+{
+	this->post(new OdometryResetEvent());
+}
+
+void OdometryViewer::clear()
+{
+	addedClouds_.clear();
+	cloudView_->clear();
+}
+
+void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
+{
+	processingData_ = true;
+	int quality = odom.info().reg.inliers;
+
+	bool lost = false;
+	bool lostStateChanged = false;
+
+	if(odom.pose().isNull())
+	{
+		UDEBUG("odom lost"); // use last pose
+		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkRed);
+		cloudView_->setBackgroundColor(Qt::darkRed);
+
+		lost = true;
+	}
+	else if(odom.info().reg.inliers>0 &&
+			qualityWarningThr_ &&
+			odom.info().reg.inliers < qualityWarningThr_)
+	{
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkYellow);
+		cloudView_->setBackgroundColor(Qt::darkYellow);
+	}
+	else
+	{
+		UDEBUG("odom ok");
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
+		cloudView_->setBackgroundColor(Qt::black);
+	}
+
+	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
+
+	if(cloudShown_->isChecked() &&
+		!odom.data().imageRaw().empty() &&
+		!odom.data().depthOrRightRaw().empty() &&
+		(odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
+	{
+		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
+
+		if(!odom.data().depthRaw().empty())
+		{
+			if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
+			   odom.data().imageRaw().rows % decimationSpin_->value() == 0)
+			{
+				validDecimationValue_ = decimationSpin_->value();
+			}
+			else
+			{
+				UWARN("Decimation (%d) must be a denominator of the width and height of "
+						"the image (%d/%d). Using last valid decimation value (%d).",
+						decimationSpin_->value(),
+						odom.data().imageRaw().cols,
+						odom.data().imageRaw().rows,
+						validDecimationValue_);
+			}
+		}
+		else
+		{
+			validDecimationValue_ = decimationSpin_->value();
+		}
+
+		// visualization: buffering the clouds
+		// Create the new cloud
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::IndicesPtr validIndices(new std::vector<int>);
+		cloud = util3d::cloudRGBFromSensorData(
+				odom.data(),
+				validDecimationValue_,
+				0,
+				0,
+				validIndices.get(),
+				parameters_);
+
+		if(voxelSpin_->value())
+		{
+			cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
+		}
+
+		if(cloud->size())
+		{
+			if(!odom.pose().isNull())
+			{
+				if(cloudView_->getAddedClouds().contains("cloudtmp"))
+				{
+					cloudView_->removeCloud("cloudtmp");
+				}
+
+				while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
+				{
+					UASSERT(cloudView_->removeCloud(addedClouds_.first()));
+					addedClouds_.pop_front();
+				}
+
+				odom.data().id()?id_=odom.data().id():++id_;
+				std::string cloudName = uFormat("cloud%d", id_);
+				addedClouds_.push_back(cloudName);
+				UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
+			}
+			else
+			{
+				cloudView_->addCloud("cloudtmp", cloud, lastOdomPose_);
+			}
+		}
+	}
+	else if(!cloudShown_->isChecked())
+	{
+		while(!addedClouds_.empty())
+		{
+			UASSERT(cloudView_->removeCloud(addedClouds_.first()));
+			addedClouds_.pop_front();
+		}
+	}
+
+	if(!odom.pose().isNull())
+	{
+		lastOdomPose_ = odom.pose();
+		cloudView_->updateCameraTargetPosition(odom.pose());
+
+		if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+		{
+			cloudView_->updateCameraFrustums(odom.pose(), odom.data().cameraModels());
+		}
+		else if(odom.data().stereoCameraModels().size() && !odom.data().stereoCameraModels()[0].localTransform().isNull())
+		{
+			cloudView_->updateCameraFrustums(odom.pose(), odom.data().stereoCameraModels());
+		}
+	}
+
+	if(scanShown_->isChecked())
+	{
+		// scan local map
+		if(!odom.info().localScanMap.isEmpty())
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
+			if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
+			{
+				UERROR("Adding scanMapOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanMapOdom", true);
+				cloudView_->setCloudOpacity("scanMapOdom", 0.5);
+			}
+		}
+		// scan cloud
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			LaserScan scan = odom.data().laserScanRaw();
+
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose() * scan.localTransform());
+
+			if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
+			{
+				UERROR("Adding scanOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanOdom", true);
+				cloudView_->setCloudOpacity("scanOdom", 0.5);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("scanMapOdom");
+		cloudView_->removeCloud("scanOdom");
+	}
+
+	// 3d features
+	if(featuresShown_->isChecked())
+	{
+		if(!odom.info().localMap.empty() && !odom.pose().isNull())
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			cloud->resize(odom.info().localMap.size());
+			int i=0;
+			for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
+			{
+				// filter very far features from current location
+				if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
+				{
+					(*cloud)[i].x = iter->second.x;
+					(*cloud)[i].y = iter->second.y;
+					(*cloud)[i].z = iter->second.z;
+
+					// green = inlier, yellow = outliers
+					bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
+					(*cloud)[i].r = inlier?0:255;
+					(*cloud)[i].g = 255;
+					(*cloud)[i++].b = 0;
+				}
+			}
+			cloud->resize(i);
+
+			if(!cloudView_->addCloud("featuresOdom", cloud))
+			{
+				UERROR("Adding featuresOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("featuresOdom", true);
+				cloudView_->setCloudPointSize("featuresOdom", 3);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("featuresOdom");
+	}
+
+	if(!odom.data().imageRaw().empty())
+	{
+		if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
+		{
+			imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
+		}
+		else if(odom.info().type == (int)Odometry::kTypeF2F ||
+				odom.info().type == (int)Odometry::kTypeViso2 ||
+				odom.info().type == (int)Odometry::kTypeFovis ||
+				odom.info().type == (int)Odometry::kTypeMSCKF)
+		{
+			std::vector<cv::KeyPoint> kpts;
+			cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
+			imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
+		}
+
+		imageView_->clearLines();
+		if(lost)
+		{
+			if(lostStateChanged)
+			{
+				// save state
+				odomImageShow_ = imageView_->isImageShown();
+				odomImageDepthShow_ = imageView_->isImageDepthShown();
+			}
+			imageView_->setImageDepth(odom.data().imageRaw());
+			imageView_->setImageShown(true);
+			imageView_->setImageDepthShown(true);
+		}
+		else
+		{
+			if(lostStateChanged)
+			{
+				// restore state
+				imageView_->setImageShown(odomImageShow_);
+				imageView_->setImageDepthShown(odomImageDepthShow_);
+			}
+
+			imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
+			if(imageView_->isImageDepthShown())
+			{
+				imageView_->setImageDepth(odom.data().depthOrRightRaw());
+			}
+
+			if( odom.info().type == Odometry::kTypeF2M ||
+				odom.info().type == (int)Odometry::kTypeORBSLAM ||
+				odom.info().type == (int)Odometry::kTypeMSCKF)
+			{
+				if(imageView_->isFeaturesShown())
+				{
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
+					{
+						imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
+					}
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
+					{
+						imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
+					}
+				}
+			}
+		}
+		if((odom.info().type == (int)Odometry::kTypeF2F ||
+			odom.info().type == (int)Odometry::kTypeViso2 ||
+			odom.info().type == (int)Odometry::kTypeFovis) && odom.info().cornerInliers.size())
+		{
+			if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
+			{
+				//draw lines
+				UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
+				for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
+				{
+					if(imageView_->isFeaturesShown())
+					{
+						imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
+					}
+					if(imageView_->isLinesShown())
+					{
+						imageView_->addLine(
+								odom.info().newCorners[odom.info().cornerInliers[i]].x,
+								odom.info().newCorners[odom.info().cornerInliers[i]].y,
+								odom.info().refCorners[odom.info().cornerInliers[i]].x,
+								odom.info().refCorners[odom.info().cornerInliers[i]].y,
+								Qt::blue);
+					}
+				}
+			}
+		}
+
+		if(!odom.data().imageRaw().empty())
+		{
+			imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
+		}
+	}
+
+	imageView_->update();
+	cloudView_->update();
+	cloudView_->refreshView();
+	QApplication::processEvents();
+	processingData_ = false;
+}
+
+bool OdometryViewer::handleEvent(UEvent * event)
+{
+	if(!processingData_ && this->isVisible())
+	{
+		if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
+			if(odomEvent->data().isValid())
+			{
+				processingData_ = true;
+				QMetaObject::invokeMethod(this, "processData",
+						Q_ARG(rtabmap::OdometryEvent, *odomEvent));
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/ParametersToolBox.cpp

@@ -0,0 +1,596 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+//
+// Original version from Find-Object: https://github.com/introlab/find-object
+//
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Optimizer.h>
+
+#include "rtabmap/gui/ParametersToolBox.h"
+#include <QComboBox>
+#include <QDoubleSpinBox>
+#include <QLineEdit>
+#include <QStackedWidget>
+#include <QScrollArea>
+#include <QLabel>
+#include <QGroupBox>
+#include <QCheckBox>
+#include <QVBoxLayout>
+#include <QMessageBox>
+#include <QPushButton>
+#include <stdio.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <opencv2/opencv_modules.hpp>
+
+namespace rtabmap {
+
+ParametersToolBox::ParametersToolBox(QWidget *parent) :
+	QWidget(parent),
+	comboBox_(new QComboBox(this)),
+	stackedWidget_(new QStackedWidget(this))
+{
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	this->setLayout(layout);
+
+	layout->addWidget(comboBox_);
+	layout->addWidget(stackedWidget_, 1);
+	QPushButton * resetButton = new QPushButton(this);
+	resetButton->setText(tr("Restore Defaults"));
+	layout->addWidget(resetButton);
+	connect(resetButton, SIGNAL(clicked()), this, SLOT(resetCurrentPage()));
+}
+
+ParametersToolBox::~ParametersToolBox()
+{
+}
+
+QWidget * ParametersToolBox::getParameterWidget(const QString & key)
+{
+	return this->findChild<QWidget*>(key);
+}
+
+QStringList ParametersToolBox::resetPage(int index)
+{
+	QStringList paramChanged;
+	const QObjectList & children = stackedWidget_->widget(index)->children().first()->children().first()->children();
+	for(int j=0; j<children.size();++j)
+	{
+		QString key = children.at(j)->objectName();
+		// ignore working memory
+		QString group = key.split("/").first();
+		if(parameters_.find(key.toStdString())!=parameters_.end())
+		{
+			UASSERT_MSG(parameters_.find(key.toStdString()) != parameters_.end(), uFormat("key=%s", key.toStdString().c_str()).c_str());
+			std::string value = Parameters::getDefaultParameters().at(key.toStdString());
+			parameters_.at(key.toStdString()) = value;
+
+			if(qobject_cast<QComboBox*>(children.at(j)))
+			{
+				if(((QComboBox*)children.at(j))->currentIndex() != QString::fromStdString(value).split(':').first().toInt())
+				{
+					((QComboBox*)children.at(j))->setCurrentIndex(QString::fromStdString(value).split(':').first().toInt());
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QSpinBox*>(children.at(j)))
+			{
+				if(((QSpinBox*)children.at(j))->value() != uStr2Int(value))
+				{
+					((QSpinBox*)children.at(j))->setValue(uStr2Int(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QDoubleSpinBox*>(children.at(j)))
+			{
+				if(((QDoubleSpinBox*)children.at(j))->value() != uStr2Double(value))
+				{
+					((QDoubleSpinBox*)children.at(j))->setValue(uStr2Double(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QCheckBox*>(children.at(j)))
+			{
+				if(((QCheckBox*)children.at(j))->isChecked() != uStr2Bool(value))
+				{
+					((QCheckBox*)children.at(j))->setChecked(uStr2Bool(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QLineEdit*>(children.at(j)))
+			{
+				if(((QLineEdit*)children.at(j))->text().compare(QString::fromStdString(value)) != 0)
+				{
+					((QLineEdit*)children.at(j))->setText(QString::fromStdString(value));
+					paramChanged.append(key);
+				}
+			}
+		}
+	}
+	return paramChanged;
+}
+
+void ParametersToolBox::resetCurrentPage()
+{
+	this->blockSignals(true);
+	QStringList paramChanged = this->resetPage(stackedWidget_->currentIndex());
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::resetAllPages()
+{
+	QStringList paramChanged;
+	this->blockSignals(true);
+	for(int i=0; i< stackedWidget_->count(); ++i)
+	{
+		paramChanged.append(this->resetPage(i));
+	}
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::updateParametersVisibility()
+{
+	//show/hide not used parameters
+	/*QComboBox * descriptorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_2Descriptor());
+	QComboBox * detectorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_1Detector());
+	if(descriptorBox && detectorBox)
+	{
+		QString group = Parameters::kFeature2D_2Descriptor().split('/').first();
+		QWidget * panel = 0;
+		for(int i=0; i<this->count(); ++i)
+		{
+			if(this->widget(i)->objectName().compare(group) == 0)
+			{
+				panel = this->widget(i);
+				break;
+			}
+		}
+		if(panel)
+		{
+			const QObjectList & objects = panel->children();
+			QString descriptorName = descriptorBox->currentText();
+			QString detectorName = detectorBox->currentText();
+
+			for(int i=0; i<objects.size(); ++i)
+			{
+				if(!objects[i]->objectName().isEmpty())
+				{
+					if(objects[i]->objectName().contains(descriptorName) || objects[i]->objectName().contains(detectorName))
+					{
+						((QWidget*)objects[i])->setVisible(true);
+					}
+					else if(objects[i]->objectName().contains("Fast") && detectorName == QString("ORB"))
+					{
+						((QWidget*)objects[i])->setVisible(true);	// ORB uses some FAST parameters
+					}
+					else if(!objects[i]->objectName().split('/').at(1).at(0).isDigit())
+					{
+						((QWidget*)objects[i])->setVisible(false);
+					}
+				}
+			}
+		}
+	}*/
+}
+
+void ParametersToolBox::setupUi(const ParametersMap & parameters)
+{
+	parameters_ = parameters;
+	QWidget * currentItem = 0;
+	QStringList groups;
+	for(ParametersMap::const_iterator iter=parameters.begin();
+			iter!=parameters.end();
+			++iter)
+	{
+		QStringList splitted = QString::fromStdString(iter->first).split('/');
+		QString group = splitted.first();
+
+		QString name = splitted.last();
+		if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
+		{
+			groups.push_back(group);
+			QScrollArea * area = new QScrollArea(this);
+			stackedWidget_->addWidget(area);
+			currentItem = new QWidget();
+			currentItem->setObjectName(group);
+			QVBoxLayout * layout = new QVBoxLayout(currentItem);
+			layout->setSizeConstraint(QLayout::SetMinimumSize);
+			layout->setContentsMargins(0,0,0,0);
+			layout->setSpacing(0);
+			area->setWidget(currentItem);
+
+			addParameter(layout, iter->first, iter->second);
+		}
+		else
+		{
+			addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second);
+		}
+	}
+	comboBox_->addItems(groups);
+	connect(comboBox_, SIGNAL(currentIndexChanged(int)), stackedWidget_, SLOT(setCurrentIndex(int)));
+
+	updateParametersVisibility();
+}
+
+void ParametersToolBox::updateParameter(const std::string & key, const std::string & value)
+{
+	QString group = QString::fromStdString(key).split("/").first();
+	if(parameters_.find(key) != parameters_.end())
+	{
+		parameters_.at(key) = value;
+		QWidget * widget = this->findChild<QWidget*>(key.c_str());
+		QString type = QString::fromStdString(Parameters::getType(key));
+		if(type.compare("string") == 0)
+		{
+			QString valueQt = QString::fromStdString(value);
+			if(valueQt.contains(';'))
+			{
+				// It's a list, just change the index
+				QStringList splitted = valueQt.split(':');
+				((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
+			}
+			else
+			{
+				((QLineEdit*)widget)->setText(valueQt);
+			}
+		}
+		else if(type.compare("int") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("uint") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("double") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
+		}
+		else if(type.compare("float") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
+		}
+		else if(type.compare("bool") == 0)
+		{
+			((QCheckBox*)widget)->setChecked(uStr2Bool(value));
+		}
+	}
+}
+
+void ParametersToolBox::addParameter(
+		QVBoxLayout * layout,
+		const std::string & key,
+		const std::string & value)
+{
+	std::string type = Parameters::getType(key);
+	if(type.compare("string") == 0)
+	{
+		addParameter(layout, key.c_str(), QString::fromStdString(value));
+	}
+	else if(type.compare("int") == 0 ||
+	        type.compare("uint") == 0 ||
+	        type.compare("unsigned int") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Int(value));
+	}
+	else if(type.compare("double") == 0 ||
+	        type.compare("float") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Double(value));
+	}
+	else if(type.compare("bool") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Bool(value));
+	}
+	else
+	{
+	    UWARN("Not implemented type \"%s\" for parameter \"%s\". Parameter is not added to toolbox.", type.c_str(), key.c_str());
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const QString & value)
+{
+	if(value.contains(';'))
+	{
+		QComboBox * widget = new QComboBox(this);
+		widget->setObjectName(key);
+		QStringList splitted = value.split(':');
+		widget->addItems(splitted.last().split(';'));
+
+		widget->setCurrentIndex(splitted.first().toInt());
+		connect(widget, SIGNAL(currentIndexChanged(int)), this, SLOT(changeParameter(int)));
+		addParameter(layout, key, widget);
+	}
+	else
+	{
+		QLineEdit * widget = new QLineEdit(value, this);
+		widget->setObjectName(key);
+		connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+		addParameter(layout, key, widget);
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const double & value)
+{
+	QDoubleSpinBox * widget = new QDoubleSpinBox(this);
+	int decimals = 0;
+	int decimalValue = 0;
+
+	QString str = Parameters::getDefaultParameters().at(key.toStdString()).c_str();
+	if(!str.isEmpty())
+	{
+		str.replace(',', '.');
+		QStringList items = str.split('.');
+		if(items.size() == 2)
+		{
+			decimals = items.back().length();
+			decimalValue = items.back().toInt();
+		}
+	}
+
+	double def = uStr2Double(Parameters::getDefaultParameters().at(key.toStdString()));
+	if(def<0.001 || (decimals >= 4 && decimalValue>0))
+	{
+		widget->setDecimals(5);
+		widget->setSingleStep(0.0001);
+	}
+	else if(def<0.01 || (decimals >= 3 && decimalValue>0))
+	{
+		widget->setDecimals(4);
+		widget->setSingleStep(0.001);
+	}
+	else if(def<0.1 || (decimals >= 2 && decimalValue>0))
+	{
+		widget->setDecimals(3);
+		widget->setSingleStep(0.01);
+	}
+	else if(def<1.0 || (decimals >= 1 && decimalValue>0))
+	{
+		widget->setDecimals(2);
+		widget->setSingleStep(0.1);
+	}
+	else
+	{
+		widget->setDecimals(1);
+	}
+
+
+	if(def>0.0)
+	{
+		widget->setMaximum(def*1000000.0);
+	}
+	else if(def==0.0)
+	{
+		widget->setMaximum(1000000.0);
+	}
+	else if(def<0.0)
+	{
+		widget->setMinimum(def*1000000.0);
+		widget->setMaximum(0.0);
+	}
+
+	// set minimum for selected parameters
+	if(key.compare(Parameters::kGridMinGroundHeight().c_str()) == 0 ||
+		key.compare(Parameters::kGridMaxGroundHeight().c_str()) == 0 ||
+		key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0 ||
+		key.compare(Parameters::kVisDepthMaskFloorThr().c_str()) == 0)
+	{
+		widget->setMinimum(-1000000.0);
+	}
+
+	widget->setValue(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const int & value)
+{
+	QSpinBox * widget = new QSpinBox(this);
+	int def = uStr2Int(Parameters::getDefaultParameters().at(key.toStdString()));
+
+	if(def>0)
+	{
+		widget->setMaximum(def*1000000);
+	}
+	else if(def == 0)
+	{
+		widget->setMaximum(1000000);
+	}
+	else if(def<0)
+	{
+		widget->setMinimum(def*1000000);
+		widget->setMaximum(0);
+	}
+	widget->setValue(value);
+	widget->setObjectName(key);
+
+	if(key.compare(Parameters::kVisFeatureType().c_str()) == 0)
+	{
+#ifndef RTABMAP_NONFREE
+		if(value <= 1)
+		{
+			UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
+			widget->setValue(4);
+		}
+#endif
+	}
+	if(key.compare(Parameters::kOptimizerStrategy().c_str()) == 0)
+	{
+		if(value == 0 && !Optimizer::isAvailable(Optimizer::kTypeTORO))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("TORO is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("TORO is not available, setting optimization default to g2o.");
+				widget->setValue(1);
+			}
+		}
+		if(value == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("g2o is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
+			{
+				UWARN("g2o is not available, setting optimization default to TORO.");
+				widget->setValue(0);
+			}
+		}
+		if(value == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("GTSAM is not available, setting optimization default to g2o.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
+			{
+				UWARN("GTSAM is not available, setting optimization default to TORO.");
+				widget->setValue(1);
+			}
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
+			!Optimizer::isAvailable(Optimizer::kTypeGTSAM) &&
+			!Optimizer::isAvailable(Optimizer::kTypeTORO))
+		{
+			widget->setEnabled(false);
+		}
+	}
+
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const bool & value)
+{
+	QCheckBox * widget = new QCheckBox(this);
+	widget->setChecked(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(stateChanged(int)), this, SLOT(changeParameter(int)));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout, const QString & key, QWidget * widget)
+{
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	layout->insertLayout(layout->count()-1, hLayout);
+	QString tmp = key.split('/').last();
+	QLabel * label = new QLabel(tmp, this);
+	label->setObjectName(key+"/label");
+	label->setToolTip(QString("<FONT>%1 [default=%2]</FONT>")
+			.arg(Parameters::getDescription(key.toStdString()).c_str())
+			.arg(uValue(Parameters::getDefaultParameters(), key.toStdString(), std::string("?")).c_str()));
+	label->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	hLayout->addWidget(label);
+	hLayout->addWidget(widget);
+}
+
+void ParametersToolBox::changeParameter(const QString & value)
+{
+	if(sender())
+	{
+		parameters_.at(sender()->objectName().toStdString()) = value.toStdString();
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+void ParametersToolBox::changeParameter()
+{
+	if(sender())
+	{
+		QDoubleSpinBox * doubleSpinBox = qobject_cast<QDoubleSpinBox*>(sender());
+		QSpinBox * spinBox = qobject_cast<QSpinBox*>(sender());
+		QLineEdit * lineEdit = qobject_cast<QLineEdit*>(sender());
+		if(doubleSpinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(doubleSpinBox->value());
+		}
+		else if(spinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(spinBox->value());
+		}
+		else if(lineEdit)
+		{
+			parameters_.at(sender()->objectName().toStdString()) =  lineEdit->text().toStdString();
+		}
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+void ParametersToolBox::changeParameter(const int & value)
+{
+	if(sender())
+	{
+		QStringList paramChanged;
+		QComboBox * comboBox = qobject_cast<QComboBox*>(sender());
+		QCheckBox * checkBox = qobject_cast<QCheckBox*>(sender());
+		if(comboBox)
+		{
+			QStringList items;
+			for(int i=0; i<comboBox->count(); ++i)
+			{
+				items.append(comboBox->itemText(i));
+			}
+			QString merged = QString::number(value) + QString(":") + items.join(";");
+			parameters_.at(sender()->objectName().toStdString()) = merged.toStdString();
+
+			this->updateParametersVisibility();
+		}
+		else if(checkBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uBool2Str(value==Qt::Checked?true:false);
+		}
+
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+} // namespace find_object

rtabmap/guilib/src/PdfPlot.cpp

@@ -0,0 +1,176 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/PdfPlot.h"
+#include <rtabmap/utilite/ULogger.h>
+#include "rtabmap/utilite/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
+
+namespace rtabmap {
+
+PdfPlotItem::PdfPlotItem(float dataX, float dataY, float width, int childCount) :
+	UPlotItem(dataX, dataY, width),
+	_img(0),
+	_signaturesRef(0),
+	_text(0)
+{
+	setLikelihood(dataX, dataY, childCount);
+}
+
+PdfPlotItem::~PdfPlotItem()
+{
+
+}
+
+void PdfPlotItem::setLikelihood(int id, float value, int childCount)
+{
+	if(_img && id != this->data().x())
+	{
+		delete _img;
+		_img = 0;
+	}
+	this->setData(QPointF(id, value));
+	_childCount = childCount;
+}
+
+void PdfPlotItem::showDescription(bool shown)
+{
+	if(!_text)
+	{
+		_text = new QGraphicsTextItem(this);
+		_text->setVisible(false);
+	}
+	if(shown)
+	{
+		if(!_img && _signaturesRef)
+		{
+			QImage img;
+			QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
+			if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty())
+			{
+				cv::Mat image;
+				iter.value().sensorData().uncompressDataConst(&image, 0, 0);
+				if(!image.empty())
+				{
+					img = uCvMat2QImage(image);
+					QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
+					_img = new QGraphicsPixmapItem(scaled, this);
+					_img->setVisible(false);
+				}
+			}
+		}
+
+		if(_img)
+			_text->setPos(this->mapFromScene(4+150,0));
+		else
+			_text->setPos(this->mapFromScene(4,0));
+		if(_childCount >= 0)
+		{
+			_text->setPlainText(QString("ID = %1\nValue = %2\nWeight = %3").arg(this->data().x()).arg(this->data().y()).arg(_childCount));
+		}
+		else
+		{
+			_text->setPlainText(QString("ID = %1\nValue = %2").arg(this->data().x()).arg(this->data().y()));
+		}
+		_text->setVisible(true);
+		if(_img)
+		{
+			_img->setPos(this->mapFromScene(4,0));
+			_img->setVisible(true);
+		}
+	}
+	else
+	{
+		_text->setVisible(false);
+		if(_img)
+			_img->setVisible(false);
+	}
+	UPlotItem::showDescription(shown);
+}
+
+
+
+
+
+PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef = 0, QObject * parent) :
+	UPlotCurve(name, parent),
+	_signaturesMapRef(signaturesMapRef)
+{
+
+}
+
+PdfPlotCurve::~PdfPlotCurve()
+{
+
+}
+
+void PdfPlotCurve::clear()
+{
+	UPlotCurve::clear();
+}
+
+void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap)
+{
+	ULOGGER_DEBUG("dataMap=%d, weightsMap=%d", dataMap.size(), weightsMap.size());
+	if(dataMap.size() > 0)
+	{
+		//match the size of the current data
+		int margin = int((_items.size()+1)/2) - dataMap.size();
+
+		while(margin < 0)
+		{
+			PdfPlotItem * newItem = new PdfPlotItem(0, 0, 2, 0);
+			newItem->setSignaturesRef(_signaturesMapRef);
+			this->_addValue(newItem);
+			++margin;
+		}
+
+		while(margin > 0)
+		{
+			this->removeItem(0);
+			--margin;
+		}
+
+		ULOGGER_DEBUG("itemsize=%d", _items.size());
+
+		// update values
+		QList<QGraphicsItem*>::iterator iter = _items.begin();
+		for(QMap<int, float>::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i)
+		{
+			((PdfPlotItem*)*iter)->setLikelihood(i.key(),  i.value(), weightsMap.value(i.key(), -1));
+			//2 times...
+			++iter;
+			++iter;
+		}
+
+		//reset minMax, this will force the plot to update the axes
+		this->updateMinMax();
+		Q_EMIT dataChanged(this);
+	}
+}
+
+}

rtabmap/guilib/src/PostProcessingDialog.cpp

@@ -0,0 +1,358 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/PostProcessingDialog.h"
+#include "ui_postProcessingDialog.h"
+
+#include <QPushButton>
+#include <QMessageBox>
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_PostProcessingDialog();
+	_ui->setupUi(this);
+
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) &&
+		!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
+		!Optimizer::isAvailable(Optimizer::kTypeCeres))
+	{
+		_ui->sba->setEnabled(false);
+		_ui->sba->setChecked(false);
+	}
+	else
+	{
+		if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+		{
+			_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(0);
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(1);
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeCeres))
+		{
+			_ui->comboBox_sbaType->setItemData(2, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(1);
+		}
+	}
+
+	restoreDefaults();
+
+	connect(_ui->buttonBox, SIGNAL(clicked(QAbstractButton *)), this, SLOT(closeDialog(QAbstractButton *)));
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->sba, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->intraSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->interSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
+	connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	updateVisibility();
+}
+
+PostProcessingDialog::~PostProcessingDialog()
+{
+	delete _ui;
+}
+
+void PostProcessingDialog::closeDialog ( QAbstractButton * button )
+{
+	UDEBUG("");
+
+	QDialogButtonBox::ButtonRole role = _ui->buttonBox->buttonRole(button);
+	switch(role)
+	{
+	case QDialogButtonBox::RejectRole:
+		this->reject();
+		break;
+
+	case QDialogButtonBox::AcceptRole:
+		if(validateForm())
+		{
+			this->accept();
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+bool PostProcessingDialog::validateForm()
+{
+	if(_ui->detectMoreLoopClosures->isChecked() && !this->intraSession() && !this->interSession())
+	{
+		QMessageBox::warning(this, tr("Configuration error"), tr("Intra-session and inter-session parameters cannot be both disabled at the same time. Please select one (or both)."));
+		return false;
+	}
+	return true;
+}
+
+void PostProcessingDialog::updateVisibility()
+{
+	_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+	_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+}
+
+void PostProcessingDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("detect_more_lc", this->isDetectMoreLoopClosures());
+	settings.setValue("cluster_radius", this->clusterRadius());
+	settings.setValue("cluster_angle", this->clusterAngle());
+	settings.setValue("iterations", this->iterations());
+	settings.setValue("intra_session", this->intraSession());
+	settings.setValue("inter_session", this->interSession());
+	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
+	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
+	settings.setValue("sba", this->isSBA());
+	settings.setValue("sba_iterations", this->sbaIterations());
+	settings.setValue("sba_type", this->sbaType());
+	settings.setValue("sba_variance", this->sbaVariance());
+	settings.setValue("sba_rematch_features", this->sbaRematchFeatures());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void PostProcessingDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	this->setDetectMoreLoopClosures(settings.value("detect_more_lc", this->isDetectMoreLoopClosures()).toBool());
+	this->setClusterRadius(settings.value("cluster_radius", this->clusterRadius()).toDouble());
+	this->setClusterAngle(settings.value("cluster_angle", this->clusterAngle()).toDouble());
+	this->setIterations(settings.value("iterations", this->iterations()).toInt());
+	this->setIntraSession(settings.value("intra_session", this->intraSession()).toBool());
+	this->setInterSession(settings.value("inter_session", this->interSession()).toBool());
+	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
+	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
+	this->setSBA(settings.value("sba", this->isSBA()).toBool());
+	this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
+	this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
+	this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
+	this->setSBARematchFeatures(settings.value("sba_rematch_features", this->sbaRematchFeatures()).toBool());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void PostProcessingDialog::restoreDefaults()
+{
+	setDetectMoreLoopClosures(true);
+	setClusterRadius(1);
+	setClusterAngle(30);
+	setIterations(5);
+	setIntraSession(true);
+	setInterSession(true);
+	setRefineNeighborLinks(false);
+	setRefineLoopClosureLinks(false);
+	setSBA(false);
+	setSBAIterations(20);
+	Optimizer::Type sbaType = Optimizer::kTypeG2O; // g2o
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		if(Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+		{
+			sbaType = Optimizer::kTypeCVSBA;
+		}
+		else if(Optimizer::isAvailable(Optimizer::kTypeCeres))
+		{
+			sbaType = Optimizer::kTypeCeres;
+		}
+	}
+	setSBAType(sbaType);
+	setSBAVariance(1.0);
+	setSBARematchFeatures(true);
+}
+
+void PostProcessingDialog::updateButtonBox()
+{
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
+			isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks() || isSBA());
+}
+
+bool PostProcessingDialog::isDetectMoreLoopClosures() const
+{
+	return _ui->detectMoreLoopClosures->isChecked();
+}
+
+double PostProcessingDialog::clusterRadius() const
+{
+	return _ui->clusterRadius->value();
+}
+
+double PostProcessingDialog::clusterAngle() const
+{
+	return _ui->clusterAngle->value();
+}
+
+int PostProcessingDialog::iterations() const
+{
+	return _ui->iterations->value();
+}
+
+bool PostProcessingDialog::intraSession() const
+{
+	return _ui->intraSession->isChecked();
+}
+
+bool PostProcessingDialog::interSession() const
+{
+	return _ui->interSession->isChecked();
+}
+
+bool PostProcessingDialog::isRefineNeighborLinks() const
+{
+	return _ui->refineNeighborLinks->isChecked();
+}
+
+bool PostProcessingDialog::isRefineLoopClosureLinks() const
+{
+	return _ui->refineLoopClosureLinks->isChecked();
+}
+
+bool PostProcessingDialog::isSBA() const
+{
+	return _ui->sba->isEnabled() && _ui->sba->isChecked();
+}
+
+int PostProcessingDialog::sbaIterations() const
+{
+	return _ui->sba_iterations->value();
+}
+double PostProcessingDialog::sbaVariance() const
+{
+	return _ui->sba_variance->value();
+}
+Optimizer::Type PostProcessingDialog::sbaType() const
+{
+	return _ui->comboBox_sbaType->currentIndex()==2?Optimizer::kTypeCeres:_ui->comboBox_sbaType->currentIndex()==1?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O;
+}
+bool PostProcessingDialog::sbaRematchFeatures() const
+{
+	return _ui->sba_rematchFeatures->isChecked();
+}
+
+//setters
+void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
+{
+	_ui->detectMoreLoopClosures->setChecked(on);
+}
+void PostProcessingDialog::setClusterRadius(double radius)
+{
+	_ui->clusterRadius->setValue(radius);
+}
+void PostProcessingDialog::setClusterAngle(double angle)
+{
+	_ui->clusterAngle->setValue(angle);
+}
+void PostProcessingDialog::setIterations(int iterations)
+{
+	_ui->iterations->setValue(iterations);
+}
+void PostProcessingDialog::setIntraSession(bool enabled)
+{
+	_ui->intraSession->setChecked(enabled);
+}
+void PostProcessingDialog::setInterSession(bool enabled)
+{
+	_ui->interSession->setChecked(enabled);
+}
+void PostProcessingDialog::setRefineNeighborLinks(bool on)
+{
+	_ui->refineNeighborLinks->setChecked(on);
+}
+void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
+{
+	_ui->refineLoopClosureLinks->setChecked(on);
+}
+void PostProcessingDialog::setSBA(bool on)
+{
+	_ui->sba->setChecked((
+			Optimizer::isAvailable(Optimizer::kTypeCVSBA) ||
+			Optimizer::isAvailable(Optimizer::kTypeG2O) ||
+			Optimizer::isAvailable(Optimizer::kTypeCeres)) && on);
+}
+void PostProcessingDialog::setSBAIterations(int iterations)
+{
+	_ui->sba_iterations->setValue(iterations);
+}
+void PostProcessingDialog::setSBAVariance(double variance)
+{
+	_ui->sba_variance->setValue(variance);
+}
+void PostProcessingDialog::setSBAType(Optimizer::Type type)
+{
+	if(type == Optimizer::kTypeCeres)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(2);
+	}
+	else if(type == Optimizer::kTypeCVSBA)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+	else
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+}
+void PostProcessingDialog::setSBARematchFeatures(bool value)
+{
+	_ui->sba_rematchFeatures->setChecked(value);
+}
+
+
+}

rtabmap/guilib/src/ProgressDialog.cpp

@@ -0,0 +1,189 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include <QLayout>
+#include <QProgressBar>
+#include <QTextEdit>
+#include <QLabel>
+#include <QPushButton>
+#include <QtGui/QCloseEvent>
+#include <QCheckBox>
+#include <QtCore/QTimer>
+#include <QtCore/QTime>
+#include <QScrollBar>
+#include "rtabmap/utilite/ULogger.h"
+
+namespace rtabmap {
+
+ProgressDialog::ProgressDialog(QWidget *parent, Qt::WindowFlags flags) :
+		QDialog(parent, flags),
+		_delayedClosingTime(1),
+		_canceled(false)
+{
+	_text = new QLabel(this);
+	_text->setWordWrap(true);
+	_progressBar = new QProgressBar(this);
+	_progressBar->setMaximum(1);
+	_detailedText  = new QTextEdit(this);
+	_detailedText->setReadOnly(true);
+	_detailedText->setLineWrapMode(QTextEdit::NoWrap);
+	_closeButton = new QPushButton(this);
+	_closeButton->setText("Close");
+	_cancelButton = new QPushButton(this);
+	_cancelButton->setText("Cancel");
+	_cancelButton-> setVisible(false);
+	_closeWhenDoneCheckBox = new QCheckBox(this);
+	_closeWhenDoneCheckBox->setChecked(true);
+	_closeWhenDoneCheckBox->setText("Close when done.");
+	_endMessage = "Finished!";
+	this->clear();
+	connect(_closeButton, SIGNAL(clicked()), this, SLOT(close()));
+	connect(_cancelButton, SIGNAL(clicked()), this, SLOT(cancel()));
+
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	layout->addWidget(_text);
+	layout->addWidget(_progressBar);
+	layout->addWidget(_detailedText);
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	layout->addLayout(hLayout);
+	hLayout->addWidget(_closeWhenDoneCheckBox);
+	hLayout->addStretch();
+	hLayout->addWidget(_cancelButton);
+	hLayout->addWidget(_closeButton);
+	this->setLayout(layout);
+
+	this->setModal(true);
+}
+
+ProgressDialog::~ProgressDialog()
+{
+
+}
+
+void ProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
+{
+	if(delayedClosingTimeSec >= 0)
+	{
+		_delayedClosingTime = delayedClosingTimeSec;
+	}
+	_closeWhenDoneCheckBox->setChecked(on);
+}
+
+void ProgressDialog::setCancelButtonVisible(bool visible)
+{
+	_cancelButton->setVisible(visible);
+}
+
+void ProgressDialog::appendText(const QString & text, const QColor & color)
+{
+	UDEBUG(text.toStdString().c_str());
+	_text->setText(text);
+	QString html = tr("<html><font color=\"#999999\">%1 </font><font color=\"%2\">%3</font></html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text);
+	_detailedText->append(html);
+	_detailedText->ensureCursorVisible();
+	_detailedText->horizontalScrollBar()->setSliderPosition(0);
+	_detailedText->verticalScrollBar()->setSliderPosition(_detailedText->verticalScrollBar()->maximum());
+}
+void ProgressDialog::setValue(int value)
+{
+	_progressBar->setValue(value);
+	if(value == _progressBar->maximum())
+	{
+		_text->setText(_endMessage);
+		_closeButton->setEnabled(true);
+		if(_closeWhenDoneCheckBox->isChecked() && _delayedClosingTime == 0)
+		{
+			this->close();
+		}
+		else if(_closeWhenDoneCheckBox->isChecked())
+		{
+			QTimer::singleShot(_delayedClosingTime*1000, this, SLOT(closeDialog()));
+		}
+	}
+}
+int ProgressDialog::maximumSteps() const
+{
+	return _progressBar->maximum();
+}
+void ProgressDialog::setMaximumSteps(int steps)
+{
+	_progressBar->setMaximum(steps);
+}
+
+void ProgressDialog::incrementStep(int steps)
+{
+	//incremental progress bar (if we don't know how many items will be added)
+	if(_progressBar->value() >= _progressBar->maximum()-steps)
+	{
+		_progressBar->setMaximum(_progressBar->maximum()+steps+1);
+	}
+	_progressBar->setValue(_progressBar->value()+steps);
+}
+
+void ProgressDialog::clear()
+{
+	_text->clear();
+	_detailedText->clear();
+	resetProgress();
+}
+
+void ProgressDialog::resetProgress()
+{
+	_progressBar->reset();
+	_closeButton->setEnabled(false);
+	_canceled = false;
+}
+
+void ProgressDialog::closeDialog()
+{
+	if(_closeWhenDoneCheckBox->isChecked())
+	{
+		close();
+	}
+}
+
+void ProgressDialog::closeEvent(QCloseEvent *event)
+{
+	if(_progressBar->value() == _progressBar->maximum())
+	{
+		_canceled = false;
+		event->accept();
+	}
+	else
+	{
+		event->ignore();
+	}
+}
+
+void ProgressDialog::cancel()
+{
+	_canceled = true;
+	Q_EMIT canceled();
+}
+
+}

rtabmap/guilib/src/StatsToolBox.cpp

@@ -0,0 +1,679 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/StatsToolBox.h"
+
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QGridLayout>
+#include <QMenu>
+#include <QLabel>
+#include <QToolButton>
+#include <QtCore/QChildEvent>
+#include <QtCore/QDir>
+#include <QtGui/QContextMenuEvent>
+#include <QToolBox>
+#include <QDialog>
+
+#include "rtabmap/utilite/UPlot.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal> & x, const std::vector<qreal> & y, const QString & unit, const QMenu * menu, QGridLayout * grid, QWidget * parent) :
+	QWidget(parent),
+	_button(0),
+	_name(0),
+	_value(0),
+	_unit(0),
+	_menu(0),
+	_cacheOn(cacheOn)
+{
+	this->setupUi(grid);
+	_name->setText(name);
+	if(y.size() == 1 || (y.size() > 1 && _cacheOn))
+	{
+		_value->setNum(y[y.size()-1]);
+	}
+	else if(y.size() > 1)
+	{
+		_value->setText("*");
+	}
+	if(cacheOn)
+	{
+		_x = x;
+		_y = y;
+	}
+	_unit->setText(unit);
+	this->updateMenu(menu);
+}
+
+StatItem::~StatItem()
+{
+
+}
+
+void StatItem::clearCache()
+{
+	_x.clear();
+	_y.clear();
+	_value->clear();
+}
+
+void StatItem::addValue(qreal y)
+{
+	if(_cacheOn)
+	{
+		_y.push_back(y);
+	}
+	_value->setText(QString::number(y, 'g', 3));
+	Q_EMIT valueAdded(y);
+}
+
+void StatItem::addValue(qreal x, qreal y)
+{
+	if(_cacheOn)
+	{
+		if (_x.size() && x <_x.back())
+		{
+			clearCache();
+		}
+
+		_y.push_back(y);
+		_x.push_back(x);
+	}
+
+	_value->setText(QString::number(y, 'g', 3));
+	Q_EMIT valueAdded(x,y);
+}
+
+void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal> & y)
+{
+	if(_cacheOn)
+	{
+		_x = x;
+		_y = y;
+		if(y.size())
+		{
+			_value->setNum(y[y.size()-1]);
+		}
+	}
+	else
+	{
+		_value->setText("*");
+	}
+	Q_EMIT valuesChanged(x,y);
+}
+
+QString StatItem::value() const
+{
+	return _value->text();
+}
+
+void StatItem::setupUi(QGridLayout * grid)
+{
+	_menu = new QMenu(this);
+	_menu->addMenu("Add to figure...");
+	_button = new QToolButton(this);
+	_button->setIcon(QIcon(":/images/Plot16.png"));
+	_button->setPopupMode(QToolButton::InstantPopup);
+	_button->setMenu(_menu);
+	_name = new QLabel(this);
+	_name->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	_name->setWordWrap(true);
+	_value = new QLabel(this);
+	_value->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	_unit = new QLabel(this);
+
+	if(grid)
+	{
+		int row = grid->rowCount();
+
+		//This fixes an issue where the
+		//button (used on col 0) of the first line in the
+		//toolbox couldn't be clicked
+		grid->addWidget(_button, row, 3);
+		grid->addWidget(_name, row, 0);
+		grid->addWidget(_value, row, 1);
+		grid->addWidget(_unit, row, 2);
+	}
+	else
+	{
+		QHBoxLayout * layout = new QHBoxLayout(this);
+		this->setLayout(layout);
+		layout->addWidget(_button);
+		layout->addWidget(_name);
+		layout->addWidget(_value);
+		layout->addWidget(_unit);
+		layout->addStretch();
+		layout->setContentsMargins(0,0,0,0);
+	}
+}
+
+void StatItem::setCacheOn(bool on)
+{
+	_cacheOn = on;
+	if(!on)
+	{
+		_x.clear();
+		_y.clear();
+	}
+}
+
+void StatItem::updateMenu(const QMenu * menu)
+{
+	_menu->clear();
+	QAction * action;
+	QList<QAction *> actions = menu->actions();
+	QMenu * plotMenu = _menu->addMenu("Add to figure...");
+	for(int i=0; i<actions.size(); ++i)
+	{
+		action = plotMenu->addAction(actions.at(i)->text());
+		connect(action, SIGNAL(triggered()), this, SLOT(preparePlotRequest()));
+	}
+}
+
+void StatItem::preparePlotRequest()
+{
+	QAction * action = qobject_cast<QAction*>(sender());
+	if(action)
+	{
+		Q_EMIT plotRequested(this, action->text());
+	}
+}
+
+
+
+
+
+
+
+
+StatsToolBox::StatsToolBox(QWidget * parent) :
+	QWidget(parent)
+{
+	ULOGGER_DEBUG("");
+	//Statistics in the GUI (for plotting)
+	_statBox = new QToolBox(this);
+	this->setLayout(new QVBoxLayout());
+	this->layout()->setContentsMargins(0,0,0,0);
+	this->layout()->addWidget(_statBox);
+	_statBox->layout()->setSpacing(0);
+	_plotMenu = new QMenu(this);
+	_plotMenu->addAction(tr("<New figure>"));
+	_workingDirectory = QDir::homePath();
+	_newFigureMaxItems = 0;
+}
+
+StatsToolBox::~StatsToolBox()
+{
+	closeFigures();
+}
+
+void StatsToolBox::closeFigures()
+{
+	QMap<QString, QWidget*> figuresTmp = _figures;
+	for(QMap<QString, QWidget*>::iterator iter = figuresTmp.begin(); iter!=figuresTmp.end(); ++iter)
+	{
+		iter.value()->close();
+	}
+}
+
+void StatsToolBox::setCacheOn(bool on)
+{
+	QList<StatItem *> items = _statBox->findChildren<StatItem *>();
+	for(int i=0; i<items.size(); ++i)
+	{
+		items[i]->setCacheOn(on);
+	}
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, bool cacheOn)
+{
+	std::vector<qreal> vx,vy;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheOn)
+{
+	std::vector<qreal> vx,vy(1);
+	vy[0] = y;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
+{
+	std::vector<qreal> vx(1),vy(1);
+	vx[0] = x;
+	vy[0] = y;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
+{
+	// round qreal to max 2 numbers after the dot
+	//x = (qreal(int(100*x)))/100;
+	//y = (qreal(int(100*y)))/100;
+
+	StatItem * item = _statBox->findChild<StatItem *>(statFullName);
+	if(item)
+	{
+		item->setCacheOn(cacheOn);
+		if(y.size() == 1 && x.size() == 1)
+		{
+			item->addValue(x[0], y[0]);
+		}
+		else if(y.size() == 1 && x.size() == 0)
+		{
+			item->addValue(y[0]);
+		}
+		else
+		{
+			item->setValues(x, y);
+		}
+	}
+	else
+	{
+		// statFullName format : "Grp/Name/unit"
+		QStringList list = statFullName.split('/');
+		QString grp;
+		QString name;
+		QString unit;
+		if(list.size() >= 3)
+		{
+			grp = list.at(0);
+			name = list.at(1);
+			unit = list.at(2);
+			for(int i=3; i<list.size(); ++i)
+			{
+				unit += "/" + list.at(i);
+			}
+		}
+		else if(list.size() == 2)
+		{
+			grp = list.at(0);
+			name = list.at(1);
+		}
+		else if(list.size() == 1)
+		{
+			name = list.at(0);
+		}
+		else
+		{
+			ULOGGER_WARN("A statistic has no name");
+			return;
+		}
+
+		if(grp.isEmpty())
+		{
+			grp = tr("Global");
+		}
+
+		int index = -1;
+		for(int i=0; i<_statBox->count(); ++i)
+		{
+			if(_statBox->itemText(i).compare(grp) == 0)
+			{
+				index = i;
+				break;
+			}
+		}
+
+		if(index<0)
+		{
+			QWidget * newWidget = new QWidget(_statBox);
+			index = _statBox->addItem(newWidget, grp);
+			QVBoxLayout * layout = new QVBoxLayout(newWidget);
+			newWidget->setLayout(layout);
+			QGridLayout * grid = new QGridLayout();
+			grid->setVerticalSpacing(2);
+			grid->setColumnStretch(0, 1);
+			layout->addLayout(grid);
+			layout->addStretch();
+		}
+
+		QVBoxLayout * layout = qobject_cast<QVBoxLayout *>(_statBox->widget(index)->layout());
+		if(!layout)
+		{
+			ULOGGER_ERROR("Layout is null ?!?");
+			return;
+		}
+		QGridLayout * grid = qobject_cast<QGridLayout *>(layout->itemAt(0)->layout());
+		if(!grid)
+		{
+			ULOGGER_ERROR("Layout is null ?!?");
+			return;
+		}
+
+		item = new StatItem(name, cacheOn, x, y, unit, _plotMenu, grid, _statBox->widget(index));
+		item->setObjectName(statFullName);
+
+		//layout->insertWidget(layout->count()-1, item);
+		connect(item, SIGNAL(plotRequested(const StatItem *, const QString &)), this, SLOT(plot(const StatItem *, const QString &)));
+		connect(this, SIGNAL(menuChanged(const QMenu *)), item, SLOT(updateMenu(const QMenu *)));
+	}
+}
+
+void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
+{
+	QWidget * fig = _figures.value(plotName, (QWidget*)0);
+	UPlot * plot = 0;
+	if(fig)
+	{
+		plot = fig->findChild<UPlot *>(plotName);
+	}
+	if(plot)
+	{
+		// if not already in the plot
+		if(!plot->contains(stat->objectName()))
+		{
+			UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
+			curve->setPen(plot->getRandomPenColored());
+			connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+			connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+			connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
+			if(stat->value().compare("*") == 0)
+			{
+				plot->setMaxVisibleItems(0);
+			}
+			if(!stat->yValues().empty())
+			{
+				if(stat->xValues().size() == stat->yValues().size())
+				{
+					curve->setData(stat->xValues(),stat->yValues());
+				}
+				else
+				{
+					curve->setData(stat->yValues());
+				}
+			}
+			if(!plot->addCurve(curve))
+			{
+				ULOGGER_WARN("Already added to the figure");
+			}
+			Q_EMIT figuresSetupChanged();
+		}
+		else
+		{
+			ULOGGER_WARN("Already added to the figure");
+		}
+		plot->activateWindow();
+	}
+	else
+	{
+		//Create a new plot
+		QString id = tr("Figure 0");
+		if(_plotMenu->actions().size())
+		{
+			id = _plotMenu->actions().last()->text();
+		}
+		id.replace(tr("Figure "), "");
+		QString newPlotName = QString(tr("Figure %1")).arg(id.toInt()+1);
+		//Dock
+		QDialog * figure = new QDialog(0, Qt::Window);
+		_figures.insert(newPlotName, figure);
+		QHBoxLayout * hLayout = new QHBoxLayout(figure);
+		hLayout->setContentsMargins(0,0,0,0);
+		figure->setWindowTitle(newPlotName);
+		figure->setAttribute(Qt::WA_DeleteOnClose, true);
+		connect(figure, SIGNAL(destroyed(QObject*)), this, SLOT(figureDeleted(QObject*)));
+		//Plot
+		UPlot * newPlot = new UPlot(figure);
+		newPlot->setWorkingDirectory(_workingDirectory);
+		newPlot->setMaxVisibleItems(_newFigureMaxItems);
+		newPlot->setObjectName(newPlotName);
+		hLayout->addWidget(newPlot);
+		_plotMenu->addAction(newPlotName);
+		figure->setSizeGripEnabled(true);
+
+		//Add a new curve linked to the statBox
+		UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
+		curve->setPen(newPlot->getRandomPenColored());
+		connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+		connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+		connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
+		if(stat->value().compare("*") == 0)
+		{
+			newPlot->setMaxVisibleItems(0);
+		}
+
+		if(!stat->yValues().empty())
+		{
+			if(stat->xValues().size() == stat->yValues().size())
+			{
+				curve->setData(stat->xValues(),stat->yValues());
+			}
+			else
+			{
+				curve->setData(stat->yValues());
+			}
+		}
+
+		if(!newPlot->addCurve(curve))
+		{
+			ULOGGER_ERROR("Not supposed to be here !?!");
+			delete curve;
+		}
+		figure->show();
+		Q_EMIT figuresSetupChanged();
+
+		Q_EMIT menuChanged(_plotMenu);
+	}
+}
+
+void StatsToolBox::figureDeleted(QObject * obj)
+{
+	if(obj)
+	{
+		QWidget * plot = qobject_cast<QWidget*>(obj);
+		if(plot)
+		{
+			_figures.remove(plot->windowTitle());
+			QList<QAction*> actions = _plotMenu->actions();
+			for(int i=0; i<actions.size(); ++i)
+			{
+				if(actions.at(i)->text().compare(plot->windowTitle()) == 0)
+				{
+					_plotMenu->removeAction(actions.at(i));
+					delete actions[i];
+					Q_EMIT menuChanged(_plotMenu);
+					break;
+				}
+			}
+			Q_EMIT figuresSetupChanged();
+		}
+		else
+		{
+			UERROR("");
+		}
+	}
+	else
+	{
+		UERROR("");
+	}
+}
+
+void StatsToolBox::clear()
+{
+	for (QMap<QString, QWidget*>::iterator i = _figures.begin(); i != _figures.end(); ++i)
+	{
+		QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+		if (plots.size() == 1)
+		{
+			QStringList names = plots[0]->curveNames();
+			plots[0]->clearData();
+		}
+		else
+        {
+            UERROR("");
+        }
+    }
+    if(_statBox->currentWidget())
+    {
+        QList<StatItem*> items = _statBox->currentWidget()->findChildren<StatItem*>();
+        for (int i = 0; i<items.size(); ++i)
+        {
+            items[i]->clearCache();
+        }
+    }
+}
+
+void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
+{
+	QMenu topMenu(this);
+	QMenu * menu = topMenu.addMenu(tr("Add all statistics from tab \"%1\" to...").arg(_statBox->itemText(_statBox->currentIndex())));
+	QList<QAction* > actions = _plotMenu->actions();
+	menu->addActions(actions);
+	QAction * aClearFigures = topMenu.addAction(tr("Clear all figures"));
+	QAction * action = topMenu.exec(event->globalPos());
+	QString plotName;
+	if(action)
+	{
+		if(action == aClearFigures)
+		{
+			this->clear();
+		}
+		else
+		{
+			for(int i=0; i<actions.size(); ++i)
+			{
+				if(actions.at(i) == action)
+				{
+					plotName = actions.at(i)->text();
+					break;
+				}
+			}
+		}
+	}
+
+	if(!plotName.isEmpty() && _statBox->currentWidget())
+	{
+		QList<StatItem*> items = _statBox->currentWidget()->findChildren<StatItem*>();
+		for(int i=0; i<items.size(); ++i)
+		{
+			this->plot(items.at(i), plotName);
+			if(plotName.compare(tr("<New figure>")) == 0)
+			{
+				plotName = _plotMenu->actions().last()->text();
+			}
+		}
+	}
+}
+
+void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds)
+{
+	curvesPerFigure.clear();
+	curveNames.clear();
+	curveThresholds.clear();
+	for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
+	{
+		QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+		if(plots.size() == 1)
+		{
+			QStringList names = plots[0]->curveNames();
+			curvesPerFigure.append(names.size());
+			curveNames.append(names);
+			for(int j=0; j<names.size(); ++j)
+			{
+				curveThresholds.append(plots[0]->isThreshold(names[j])?QString::number(plots[0]->getThresholdValue(names[j])):"NA");
+			}
+		}
+		else
+		{
+			UERROR("");
+		}
+	}
+}
+void StatsToolBox::addCurve(const QString & name, bool newFigure, bool cacheOn)
+{
+	StatItem * item = _statBox->findChild<StatItem *>(name);
+	if(!item)
+	{
+		this->updateStat(name, cacheOn);
+		item = _statBox->findChild<StatItem *>(name);
+	}
+
+	if(item)
+	{
+		if(newFigure)
+		{
+			this->plot(item, "");
+		}
+		else
+		{
+			this->plot(item, _plotMenu->actions().last()->text());
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Not supposed to be here...");
+	}
+}
+void StatsToolBox::addThreshold(const QString & name, qreal value)
+{
+	QString plotName = _plotMenu->actions().last()->text();
+	QWidget * fig = _figures.value(plotName, (QWidget*)0);
+	if(fig)
+	{
+		UPlot * plot = fig->findChild<UPlot *>(plotName);
+		if(plot)
+			plot->addThreshold(name, value);
+	}
+	else
+	{
+		UERROR("There are no figures, cannot add threshold \"%s\"", name.toStdString().c_str());
+	}
+}
+
+
+void StatsToolBox::setWorkingDirectory(const QString & workingDirectory)
+{
+	if(QDir(workingDirectory).exists())
+	{
+		_workingDirectory = workingDirectory;
+		for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
+		{
+			QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+			if(plots.size() == 1)
+			{
+				plots[0]->setWorkingDirectory(_workingDirectory);
+			}
+			else
+			{
+				UERROR("");
+			}
+		}
+	}
+	else
+	{
+		UWARN("The directory \"%s\" doesn't exist, using \"%s\" instead...",
+				workingDirectory.toStdString().c_str(),
+				_workingDirectory.toStdString().c_str());
+	}
+}
+
+}

rtabmap/guilib/src/chisel_conversions.h

@@ -0,0 +1,159 @@
+/*
+ * chisel_conversions.h
+ *
+ *  Created on: 2018-03-25
+ *      Author: mathieu
+ */
+
+#ifndef CHISEL_CONVERSIONS_H_
+#define CHISEL_CONVERSIONS_H_
+
+#include <rtabmap/core/CameraModel.h>
+#include <open_chisel/Chisel.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/common/transforms.h>
+
+namespace rtabmap
+{
+
+std::shared_ptr<chisel::ColorImage<unsigned char> > colorImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
+	std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
+	memcpy(imageChisel->GetMutableData(), image.data, image.total()*sizeof(unsigned char)*image.channels());
+	return imageChisel;
+}
+
+std::shared_ptr<chisel::DepthImage<float> > depthImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_32FC1);
+	std::shared_ptr<chisel::DepthImage<float> > imageChisel(new chisel::DepthImage<float>(image.cols, image.rows));
+	memcpy(imageChisel->GetMutableData(), (float*)image.data, image.total()*sizeof(float));
+	return imageChisel;
+}
+
+chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel& camera)
+{
+   chisel::PinholeCamera cameraToReturn;
+   chisel::Intrinsics intrinsics;
+   intrinsics.SetFx(camera.fx());
+   intrinsics.SetFy(camera.fy());
+   intrinsics.SetCx(camera.cx());
+   intrinsics.SetCy(camera.cy());
+   cameraToReturn.SetIntrinsics(intrinsics);
+   cameraToReturn.SetWidth(camera.imageWidth());
+   cameraToReturn.SetHeight(camera.imageHeight());
+   return cameraToReturn;
+}
+
+template<typename PointRGBT>
+chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud<PointRGBT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	chiselCloud->GetMutableColors().resize(cloud.size());
+	float byteToFloat = 1.0f / 255.0f;
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointRGBT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointRGBT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
+			rgb(0) = ptt.r * byteToFloat;
+			rgb(1) = ptt.g * byteToFloat;
+			rgb(2) = ptt.b * byteToFloat;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	chiselCloud->GetMutableColors().resize(oi);
+	return chiselCloud;
+}
+
+template<typename PointT>
+chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud<PointT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	return chiselCloud;
+}
+
+pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap& meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
+{
+	pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
+
+	if(meshMap.size())
+	{
+		bool hasColor = meshMap.begin()->second->colors.size();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		size_t v = 0;
+		for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
+		{
+			UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
+					it.second->vertices.size() == it.second->normals.size());
+			cloud->resize(cloud->size() + it.second->vertices.size());
+
+			mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);
+
+			for (unsigned int i=0;i<it.second->vertices.size(); ++i)
+			{
+				pcl::PointXYZRGBNormal & pt = cloud->at(v);
+				pt.x = it.second->vertices[i][0];
+				pt.y = it.second->vertices[i][1];
+				pt.z = it.second->vertices[i][2];
+				if(hasColor)
+				{
+					pt.r = it.second->colors[i][0] * 255.0f;
+					pt.g = it.second->colors[i][1] * 255.0f;
+					pt.b = it.second->colors[i][2] * 255.0f;
+				}
+				else
+				{
+					pt.r = r;
+					pt.g = g;
+					pt.b = b;
+				}
+				pt.normal_x = it.second->normals[i][0];
+				pt.normal_y = it.second->normals[i][1];
+				pt.normal_z = it.second->normals[i][2];
+				pcl::Vertices & polygon = mesh->polygons.at(v/3);
+				polygon.vertices.push_back(v++);
+			}
+		}
+		pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+	}
+	return mesh;
+}
+
+}
+
+
+#endif /* CHISEL_CONVERSIONS_H_ */