New classes: Link and GraphViewer (TORO graph visualization and laser scans occupancy grid)

New parameter: RGBD/ToroIterations=100
Rtabmap: added getGraph() method to get TORO poses/link constraints
Fixed ICP correspondences ratio over 1
CloudViewer: added frustum culling and camera view up z lock in render(), moving camera using arrow keys, Menu options: Camera far plane clipping, background color
DatabaseViewer: added 3D Map view and Graph view
MainWindow: Moved main widget (loop closure image status) to a dock widget, added Graph view.


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

guilib/include/rtabmap/gui/CloudViewer.h

@@ -11,12 +11,16 @@
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <QVTKWidget.h>
+#include <pcl/pcl_base.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/PolygonMesh.h>
 #include "rtabmap/core/Transform.h"
 #include <QtCore/QMap>
+#include <QtCore/QSet>
+#include <QtCore/qnamespace.h>
 
+#include <pcl/visualization/mouse_event.h>
 #include <pcl/PCLPointCloud2.h>
 
 namespace pcl {
@@ -110,12 +114,17 @@ public slots:
 	void setCloudPointSize(const std::string & id, int size);
 
 protected:
+	virtual void keyReleaseEvent(QKeyEvent * event);
+	virtual void keyPressEvent(QKeyEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 	virtual void handleAction(QAction * event);
 	QMenu * menu() {return _menu;}
 
 private:
 	void createMenu();
+	bool frustumCulling(const pcl::PointXYZ & cloud);
+	pcl::IndicesPtr frustumCulling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+	void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void);
 
 private:
     pcl::visualization::PCLVisualizer * _visualizer;
@@ -127,12 +136,16 @@ private:
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;
     QAction * _aShowGrid;
+    QAction * _aSetBackgroundColor;
+    QAction * _setFarPlaneDistance;
     QMenu * _menu;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
     QMap<std::string, Transform> _addedClouds; // include meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;
+    float _farPlaneDistance;
+    QSet<Qt::Key> _keysPressed;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -31,6 +31,8 @@
 #include <opencv2/features2d/features2d.hpp>
 #include <set>
 
+#include <rtabmap/core/Link.h>
+
 class Ui_DatabaseViewer;
 class QGraphicsScene;
 class QGraphicsView;
@@ -55,12 +57,18 @@ private slots:
 	void openDatabase();
 	void generateGraph();
 	void exportDatabase();
+	void extractImages();
 	void generateLocalGraph();
+	void view3DMap();
 	void generate3DMap();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);
 	void sliderBMoved(int);
+	void sliderNeighborValueChanged(int);
+	void sliderLoopValueChanged(int);
+	void sliderIterationsValueChanged(int);
+	void updateGraphView();
 
 private:
 	void updateIds();
@@ -71,12 +79,20 @@ private:
 				QLabel * labelChildren,
 				rtabmap::ImageView * view,
 				QLabel * labelId);
+	void updateConstraintView(const rtabmap::Link & link);
 
 private:
 	Ui_DatabaseViewer * ui_;
 	QList<int> ids_;
+	QMap<int, int> idToIndex_;
+	QList<rtabmap::Link> neighborLinks_;
+	QList<rtabmap::Link> loopLinks_;
 	rtabmap::Memory * memory_;
 	QString pathDatabase_;
+	std::list<std::map<int, rtabmap::Transform> > graphes_;
+	std::map<int, rtabmap::Transform> poses_;
+	std::multimap<int, rtabmap::Link> links_;
+	std::map<int, std::vector<unsigned char> > scans_;
 };
 
 #endif /* DATABASEVIEWER_H_ */

guilib/include/rtabmap/gui/MainWindow.h

@@ -116,11 +116,13 @@ private slots:
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void downloadAllClouds();
+	void downloadPoseGraph();
 	void clearTheCache();
 	void saveFigures();
 	void loadFigures();
 	void openPreferences();
 	void selectScreenCaptureFormat(bool checked);
+	void takeScreenshot();
 	void updateElapsedTime();
 	void processOdometry(const rtabmap::Image & data);
 	void applyAllPrefSettings();
@@ -214,7 +216,7 @@ private:
 
 	QMap<int, std::vector<unsigned char> > _imagesMap;
 	QMap<int, std::vector<unsigned char> > _depthsMap;
-	QMap<int, std::vector<unsigned char> > _depths2DMap;
+	std::map<int, std::vector<unsigned char> > _depths2DMap;
 	QMap<int, float> _depthConstantsMap;
 	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
@@ -234,7 +236,7 @@ private:
 
 	QString _graphSavingFileName;
 	QString _toroSavingFileName;
-	QString _autoScreenCaptureFormat;
+	bool _autoScreenCaptureOdomSync;
 
 	QVector<int> _refIds;
 	QVector<int> _loopClosureIds;

guilib/src/CMakeLists.txt

@@ -18,6 +18,7 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
 	./ExportDialog.h
 	./MapVisibilityWidget.h
+	./GraphViewer.h
 )
 
 SET(uis
@@ -65,6 +66,7 @@ SET(SRC_FILES
 	./DataRecorder.cpp
 	./ExportDialog.cpp
 	./MapVisibilityWidget.cpp
+	./GraphViewer.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}

guilib/src/CloudViewer.cpp

@@ -8,19 +8,33 @@
 #include "rtabmap/gui/CloudViewer.h"
 
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/util3d.h>
 #include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/filters/frustum_culling.h>
 #include <QtGui/QMenu>
 #include <QtGui/QAction>
 #include <QtGui/QContextMenuEvent>
 #include <QtGui/QInputDialog>
 #include <QtGui/QWheelEvent>
+#include <QtGui/QKeyEvent>
+#include <QtGui/QColorDialog>
+#include <set>
 
 #include <vtkRenderWindow.h>
 
 namespace rtabmap {
 
+void CloudViewer::mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void)
+{
+	if (event.getButton () == pcl::visualization::MouseEvent::LeftButton ||
+	  event.getButton () == pcl::visualization::MouseEvent::MiddleButton)
+	{
+		this->render(); // this will apply frustum
+	}
+}
+
 CloudViewer::CloudViewer(QWidget *parent) :
 		QVTKWidget(parent),
 #if defined(WIN32) || defined(__APPLE__)
@@ -36,10 +50,13 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aSetTrajectorySize(0),
 		_aClearTrajectory(0),
 		_aShowGrid(0),
+		_aSetBackgroundColor(0),
+		_setFarPlaneDistance(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
-		_lastPose(Transform::getIdentity())
+		_lastPose(Transform::getIdentity()),
+		_farPlaneDistance(10000)
 {
 	this->setMinimumSize(200, 200);
 
@@ -49,6 +66,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 	_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
 #endif
 
+	_visualizer->registerMouseCallback (&CloudViewer::mouseEventOccurred, *this, (void*)_visualizer);
 	_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
@@ -61,7 +79,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 CloudViewer::~CloudViewer()
 {
 	UDEBUG("");
-	//_visualizer->close();
+	this->removeAllClouds();
 	delete _visualizer;
 }
 
@@ -87,6 +105,8 @@ void CloudViewer::createMenu()
 	_aClearTrajectory = new QAction("Clear trajectory", this);
 	_aShowGrid = new QAction("Show grid", this);
 	_aShowGrid->setCheckable(true);
+	_aSetBackgroundColor = new QAction("Set background color...", this);
+	_setFarPlaneDistance = new QAction("Set far plane distance...", this);
 
 	QMenu * cameraMenu = new QMenu("Camera", this);
 	cameraMenu->addAction(_aLockCamera);
@@ -94,6 +114,7 @@ void CloudViewer::createMenu()
 	cameraMenu->addAction(freeCamera);
 	cameraMenu->addSeparator();
 	cameraMenu->addAction(_aLockViewZ);
+	cameraMenu->addAction(_setFarPlaneDistance);
 	cameraMenu->addAction(_aResetCamera);
 	QActionGroup * group = new QActionGroup(this);
 	group->addAction(_aLockCamera);
@@ -110,6 +131,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
 	_menu->addAction(_aShowGrid);
+	_menu->addAction(_aSetBackgroundColor);
 }
 
 bool CloudViewer::updateCloudPose(
@@ -430,13 +452,6 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 				cameras.front().view[2] = Fp[10];
 			}
 
-			if(_aLockViewZ->isChecked())
-			{
-				cameras.front().view[0] = 0;
-				cameras.front().view[1] = 0;
-				cameras.front().view[2] = 1;
-			}
-
 			_visualizer->setCameraPosition(
 					cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 					cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
@@ -451,9 +466,98 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 
 void CloudViewer::render()
 {
+	// camera view up z locked?
+	if(_aLockViewZ->isChecked())
+	{
+		std::vector<pcl::visualization::Camera> cameras;
+		_visualizer->getCameras(cameras);
+
+		cameras.front().view[0] = 0;
+		cameras.front().view[1] = 0;
+		cameras.front().view[2] = 1;
+
+		_visualizer->setCameraPosition(
+			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+
+	}
+
+	// frustum
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(_addedClouds.size());
+	int i=0;
+	for(QMap<std::string, Transform>::iterator iter = _addedClouds.begin(); iter!=_addedClouds.end(); ++iter)
+	{
+		(*cloud)[i++] = pcl::PointXYZ(iter.value().x(), iter.value().y(), iter.value().z());
+	}
+
+	pcl::IndicesPtr indices = frustumCulling(cloud);
+	std::set<int> visibleClouds(indices->begin(), indices->end());
+	i=0;
+	for(QMap<std::string, Transform>::iterator iter = _addedClouds.begin(); iter!=_addedClouds.end(); ++iter)
+	{
+		this->setCloudVisibility(iter.key(), visibleClouds.find(i) != visibleClouds.end());
+		++i;
+	}
+
 	this->GetRenderWindow()->Render();
 }
 
+bool CloudViewer::frustumCulling(const pcl::PointXYZ & point)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->push_back(point);
+	pcl::IndicesPtr indices = frustumCulling(cloud);
+	return indices->size() != 0;
+}
+
+pcl::IndicesPtr CloudViewer::frustumCulling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
+{
+	pcl::IndicesPtr indices(new std::vector<int>());
+	if(cloud->size())
+	{
+		if(_farPlaneDistance)
+		{
+			std::vector<pcl::visualization::Camera> cameras;
+			_visualizer->getCameras(cameras);
+
+			Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
+														  cameras.front().focal[1] - cameras.front().pos[1],
+														  cameras.front().focal[2] - cameras.front().pos[2]).normalized();
+			Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+			Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
+			Eigen::Vector3f xAxis = vPosToFocal.normalized();
+			zAxis = xAxis.cross(yAxis);
+			Transform PR(xAxis[0], yAxis[0], zAxis[0],0,
+						 xAxis[1], yAxis[1], zAxis[1],0,
+						 xAxis[2], yAxis[2], zAxis[2],0);
+			Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
+						PR[4], PR[5], PR[6], cameras.front().pos[1],
+						PR[8], PR[9], PR[10], cameras.front().pos[2]);
+
+			pcl::FrustumCulling<pcl::PointXYZ> fc;
+			fc.setInputCloud (cloud);
+			fc.setNearPlaneDistance (0.0);
+			fc.setFarPlaneDistance (_farPlaneDistance);
+			//float fov = cameras.front().fovy*180.0f/3.14159265359;
+			fc.setVerticalFOV (52);
+			fc.setHorizontalFOV (52);
+			fc.setCameraPose (util3d::transformToEigen4f(P));
+			fc.filter (*indices);
+		}
+		else
+		{
+			indices->resize(cloud->size());
+			for(unsigned int i=0; i<indices->size(); ++i)
+			{
+				indices->at(i) = i;
+			}
+		}
+	}
+	return indices;
+}
+
 void CloudViewer::setBackgroundColor(const QColor & color)
 {
 	_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
@@ -508,6 +612,96 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size)
 	}
 }
 
+
+
+void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
+	if(event->key() == Qt::Key_Up ||
+		event->key() == Qt::Key_Down ||
+		event->key() == Qt::Key_Left ||
+		event->key() == Qt::Key_Right)
+	{
+		_keysPressed -= (Qt::Key)event->key();
+	}
+	else
+	{
+		QVTKWidget::keyPressEvent(event);
+	}
+}
+
+void CloudViewer::keyPressEvent(QKeyEvent * event)
+{
+	if(event->key() == Qt::Key_Up ||
+		event->key() == Qt::Key_Down ||
+		event->key() == Qt::Key_Left ||
+		event->key() == Qt::Key_Right)
+	{
+		_keysPressed += (Qt::Key)event->key();
+
+		std::vector<pcl::visualization::Camera> cameras;
+		_visualizer->getCameras(cameras);
+
+		//update camera position
+		Eigen::Vector3f pos(cameras.front().pos[0], cameras.front().pos[1], _aLockViewZ->isChecked()?0:cameras.front().pos[2]);
+		Eigen::Vector3f focal(cameras.front().focal[0], cameras.front().focal[1], _aLockViewZ->isChecked()?0:cameras.front().focal[2]);
+		Eigen::Vector3f viewUp(0, 0, 1);
+		Eigen::Vector3f cummulatedDir(0,0,0);
+		float step = 0.2f;
+		if(_keysPressed.contains(Qt::Key_Up))
+		{
+			Eigen::Vector3f dir;
+			if(event->modifiers() & Qt::ShiftModifier)
+			{
+				dir = viewUp * step;// up
+			}
+			else
+			{
+				dir = (focal-pos).normalized() * step; // forward
+			}
+			cummulatedDir += dir;
+		}
+		if(_keysPressed.contains(Qt::Key_Down))
+		{
+			Eigen::Vector3f dir;
+			if(event->modifiers() & Qt::ShiftModifier)
+			{
+				dir = viewUp * -step;// down
+			}
+			else
+			{
+				dir = (focal-pos).normalized() * -step; // backward
+			}
+			cummulatedDir += dir;
+		}
+		if(_keysPressed.contains(Qt::Key_Right))
+		{
+			Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step; // strafing right
+			cummulatedDir += dir;
+		}
+		if(_keysPressed.contains(Qt::Key_Left))
+		{
+			Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step; // strafing left
+			cummulatedDir += dir;
+		}
+
+		cameras.front().pos[0] += cummulatedDir[0];
+		cameras.front().pos[1] += cummulatedDir[1];
+		cameras.front().pos[2] += cummulatedDir[2];
+		cameras.front().focal[0] += cummulatedDir[0];
+		cameras.front().focal[1] += cummulatedDir[1];
+		cameras.front().focal[2] += cummulatedDir[2];
+		_visualizer->setCameraPosition(
+			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+
+		render();
+	}
+	else
+	{
+		QVTKWidget::keyPressEvent(event);
+	}
+}
+
 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QAction * a = _menu->exec(event->globalPos());
@@ -540,6 +734,7 @@ void CloudViewer::handleAction(QAction * a)
 				-1, 0, 0,
 				0, 0, 0,
 				0, 0, 1);
+		_farPlaneDistance = 10000;
 		this->render();
 	}
 	else if(a == _aShowGrid)
@@ -578,6 +773,21 @@ void CloudViewer::handleAction(QAction * a)
 
 		this->render();
 	}
+	else if(a == _aSetBackgroundColor)
+	{
+		QColor color = Qt::black;
+		color = QColorDialog::getColor(color, this);
+		this->setBackgroundColor(color);
+	}
+	else if(a == _setFarPlaneDistance)
+	{
+		bool ok;
+		double distance = QInputDialog::getDouble(this, tr("Far plane distance"), tr("Clipping plane distance"), _farPlaneDistance, 1, 10000, 0, &ok);
+		if(ok)
+		{
+			_farPlaneDistance = distance;
+		}
+	}
 }
 
 } /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -18,6 +18,7 @@
  */
 
 #include "rtabmap/gui/DatabaseViewer.h"
+#include "rtabmap/gui/CloudViewer.h"
 #include "ui_DatabaseViewer.h"
 #include <QtGui/QMessageBox>
 #include <QtGui/QFileDialog>
@@ -28,6 +29,7 @@
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core/core_c.h>
+#include <opencv2/highgui/highgui.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
@@ -58,14 +60,22 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	ui_ = new Ui_DatabaseViewer();
 	ui_->setupUi(this);
 
+	ui_->dockWidget_constraints->setVisible(false);
+	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
+	ui_->dockWidget_graphView->setVisible(false);
+	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
+	connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
+
 	connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
 	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
 
 	// connect actions with custom slots
 	connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
 	connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
+	connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
 	connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
 	connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
+	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 
 	ui_->horizontalSlider_A->setTracking(false);
@@ -76,6 +86,24 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
 	connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
 	connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
+
+	ui_->horizontalSlider_neighbors->setTracking(false);
+	ui_->horizontalSlider_loops->setTracking(false);
+	ui_->horizontalSlider_neighbors->setEnabled(false);
+	ui_->horizontalSlider_loops->setEnabled(false);
+	connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
+	connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
+	connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
+	connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
+
+	ui_->horizontalSlider_iterations->setTracking(false);
+	ui_->dockWidget_graphView->setEnabled(false);
+	connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
+	connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
+	connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
+	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
+	connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+
 }
 
 DatabaseViewer::~DatabaseViewer()
@@ -114,6 +142,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 		std::string driverType = "sqlite3";
 		rtabmap::ParametersMap parameters;
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
 
 		memory_ = new rtabmap::Memory();
 
@@ -211,6 +240,30 @@ void DatabaseViewer::exportDatabase()
 	}
 }
 
+void DatabaseViewer::extractImages()
+{
+	if(!memory_ || ids_.size() == 0)
+	{
+		return;
+	}
+
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
+	if(!path.isNull())
+	{
+		for(int i=0; i<ids_.size(); i+=1)
+		{
+			int id = ids_.at(i);
+			std::vector<unsigned char> compressedRgb = memory_->getImage(id);
+			if(compressedRgb.size())
+			{
+				cv::Mat imageMat = rtabmap::util3d::uncompressImage(compressedRgb);
+				cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
+				UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str());
+			}
+		}
+	}
+}
+
 void DatabaseViewer::updateIds()
 {
 	if(!memory_)
@@ -220,6 +273,43 @@ void DatabaseViewer::updateIds()
 
 	std::set<int> ids = memory_->getAllSignatureIds();
 	ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
+	idToIndex_.clear();
+	for(int i=0; i<ids_.size(); ++i)
+	{
+		idToIndex_.insert(ids_[i], i);
+	}
+
+	poses_.clear();
+	links_.clear();
+	if(memory_->getLastWorkingSignature())
+	{
+		std::map<int, int> nids = memory_->getNeighborsId(memory_->getLastWorkingSignature()->id(), 0, -1, true);
+		memory_->getMetricConstraints(uKeys(nids), poses_, links_, true);
+
+		ui_->spinBox_optimizationsFrom->setRange(1, memory_->getLastWorkingSignature()->id());
+		ui_->spinBox_optimizationsFrom->setValue(memory_->getLastWorkingSignature()->id());
+	}
+
+	neighborLinks_.clear();
+	loopLinks_.clear();
+	for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
+	{
+		if(!iter->second.transform().isNull())
+		{
+			if(iter->second.type() == rtabmap::Link::kNeighbor)
+			{
+				neighborLinks_.append(iter->second);
+			}
+			else
+			{
+				loopLinks_.append(iter->second);
+			}
+		}
+		else
+		{
+			UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
+		}
+	}
 
 	UINFO("Loaded %d ids", ids_.size());
 
@@ -243,6 +333,28 @@ void DatabaseViewer::updateIds()
 		ui_->label_idA->setText("NaN");
 		ui_->label_idB->setText("NaN");
 	}
+	if(neighborLinks_.size())
+	{
+		ui_->horizontalSlider_neighbors->setMinimum(0);
+		ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
+		ui_->horizontalSlider_neighbors->setEnabled(true);
+		ui_->horizontalSlider_neighbors->setSliderPosition(0);
+	}
+	else
+	{
+		ui_->horizontalSlider_neighbors->setEnabled(false);
+	}
+	if(loopLinks_.size())
+	{
+		ui_->horizontalSlider_loops->setMinimum(0);
+		ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
+		ui_->horizontalSlider_loops->setEnabled(true);
+		ui_->horizontalSlider_loops->setSliderPosition(0);
+	}
+	else
+	{
+		ui_->horizontalSlider_loops->setEnabled(false);
+	}
 }
 
 void DatabaseViewer::generateGraph()
@@ -301,7 +413,7 @@ void DatabaseViewer::generateLocalGraph()
 	}
 }
 
-void DatabaseViewer::generate3DMap()
+void DatabaseViewer::view3DMap()
 {
 	if(!ids_.size() || !memory_)
 	{
@@ -313,32 +425,58 @@ void DatabaseViewer::generate3DMap()
 
 	if(ok)
 	{
-		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
 		if(ok)
 		{
-			float voxelSize = QInputDialog::getDouble(this, tr("Voxel size?"), tr("Voxel Size"), 0.01, 0, 0.1, 3, &ok);
+			QStringList items;
+			items.append("1");
+			items.append("2");
+			items.append("4");
+			items.append("8");
+			items.append("16");
+			QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 			if(ok)
 			{
-				QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Map" + QString::number(id) + ".pcd", tr("PCL file (*.pcd)"));
-				if(!path.isEmpty())
+				int decimation = item.toInt();
+				double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
+				if(ok)
 				{
 					std::map<int, int> ids = memory_->getNeighborsId(id, margin, -1, false);
 					if(ids.size() > 0)
 					{
+						rtabmap::DetailedProgressDialog progressDialog(this);
+						progressDialog.setMaximumSteps(ids.size()+2);
+						progressDialog.show();
+
+						progressDialog.appendText("Graph generation...");
 						std::map<int, rtabmap::Transform> poses, optimizedPoses;
-						std::multimap<int, std::pair<int, rtabmap::Transform> > edgeConstraints;
+						std::multimap<int, rtabmap::Link> edgeConstraints;
 						memory_->getMetricConstraints(uKeys(ids), poses, edgeConstraints, true);
+						progressDialog.appendText("Graph generation... done!");
+						progressDialog.incrementStep();
 
-						UINFO("Poses=%d, constraints=%d", poses.size(), edgeConstraints.size());
-
-						rtabmap::util3d::saveTOROGraph("toro1.graph", poses, edgeConstraints);
-
+						progressDialog.appendText("Graph optimization...");
 						rtabmap::Transform mapCorrection;
-						rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, 100, optimizedPoses, mapCorrection);
+						rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, mapCorrection, 100, true);
+						progressDialog.appendText("Graph optimization... done!");
+						progressDialog.incrementStep();
 
-						rtabmap::util3d::saveTOROGraph("toro2.graph", optimizedPoses, edgeConstraints);
+						// create a window
+						QWidget * window = new QWidget(this, Qt::Popup);
+						window->setAttribute(Qt::WA_DeleteOnClose);
+						window->setWindowFlags(Qt::Dialog);
+						window->setWindowTitle(tr("3D Map"));
+						window->setMinimumWidth(800);
+						window->setMinimumHeight(600);
+
+						rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
+
+						QVBoxLayout *layout = new QVBoxLayout();
+						layout->addWidget(viewer);
+						window->setLayout(layout);
+
+						window->showNormal();
 
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 						for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 						{
 							rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
@@ -355,35 +493,129 @@ void DatabaseViewer::generate3DMap()
 										imageMat,
 										depthMat,
 										depthMat.cols/2, depthMat.rows/2,
-										1.0f/depthConstant, 1.0f/depthConstant);
+										1.0f/depthConstant, 1.0f/depthConstant,
+										decimation);
 
-								if(voxelSize > 0.0f)
+								if(maxDepth)
 								{
-									cloud = rtabmap::util3d::voxelize(cloud, voxelSize);
+									cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
 								}
 
-								cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+								cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
 
-								*assembledCloud += *cloud;
+								viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose);
+
+								UINFO("Generated %d (%d points)", iter->first, cloud->size());
+								progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
+								progressDialog.incrementStep();
+								QApplication::processEvents();
 							}
 						}
-
-						if(voxelSize > 0.0f)
-						{
-							pcl::VoxelGrid<pcl::PointXYZRGB> filter;
-							filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-							filter.setInputCloud(assembledCloud);
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
-							filter.filter(*tmp);
-							assembledCloud = tmp;
-						}
-
-						pcl::io::savePCDFile(path.toStdString(), *assembledCloud, true);
-						QMessageBox::information(this, "Generated Map", tr("Map saved to %1!\n(%2 nodes, %3 points)").arg(path).arg(ids.size()).arg(assembledCloud->size()));
+						progressDialog.setValue(progressDialog.maximumSteps());
 					}
 					else
 					{
-						QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
+						QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(id));
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void DatabaseViewer::generate3DMap()
+{
+	if(!ids_.size() || !memory_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
+
+	if(ok)
+	{
+		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
+		if(ok)
+		{
+			QStringList items;
+			items.append("1");
+			items.append("2");
+			items.append("4");
+			items.append("8");
+			items.append("16");
+			QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
+			if(ok)
+			{
+				int decimation = item.toInt();
+				double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
+				if(ok)
+				{
+					QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
+					if(!path.isEmpty())
+					{
+						std::map<int, int> ids = memory_->getNeighborsId(id, margin, -1, false);
+						if(ids.size() > 0)
+						{
+							rtabmap::DetailedProgressDialog progressDialog;
+							progressDialog.setMaximumSteps(ids.size()+2);
+							progressDialog.show();
+
+							progressDialog.appendText("Graph generation...");
+							std::map<int, rtabmap::Transform> poses, optimizedPoses;
+							std::multimap<int, rtabmap::Link> edgeConstraints;
+							memory_->getMetricConstraints(uKeys(ids), poses, edgeConstraints, true);
+							progressDialog.appendText("Graph generation... done!");
+							progressDialog.incrementStep();
+
+							progressDialog.appendText("Graph optimization...");
+							rtabmap::Transform mapCorrection;
+							rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, mapCorrection, 100, true);
+							progressDialog.appendText("Graph optimization... done!");
+							progressDialog.incrementStep();
+
+							for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+							{
+								rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
+								if(!pose.isNull())
+								{
+									std::vector<unsigned char> image, depth, depth2d;
+									float depthConstant;
+									rtabmap::Transform localTransform;
+									memory_->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
+									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+									cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
+									cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+									cloud = rtabmap::util3d::cloudFromDepthRGB(
+											imageMat,
+											depthMat,
+											depthMat.cols/2, depthMat.rows/2,
+											1.0f/depthConstant, 1.0f/depthConstant,
+											decimation);
+
+									if(maxDepth)
+									{
+										cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
+									}
+
+									cloud = rtabmap::util3d::transformPointCloud(cloud, pose*localTransform);
+									std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
+									pcl::io::savePCDFile(name, *cloud);
+									UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
+									progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
+									progressDialog.incrementStep();
+									QApplication::processEvents();
+								}
+							}
+							progressDialog.setValue(progressDialog.maximumSteps());
+
+							QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(ids.size()).arg(path));
+						}
+						else
+						{
+							QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(id));
+						}
 					}
 				}
 			}
@@ -444,10 +676,6 @@ void DatabaseViewer::update(int value,
 				memory_->getImageDepth(id, image, depth, depth2d, depthConstant, localTransform);
 				cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 				cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
-				UINFO("loaded image(%d/%d) depth(%d/%d) depthConstant(%f)",
-						imageMat.cols, imageMat.rows,
-						depthMat.cols, depthMat.rows,
-						depthConstant);
 				if(!image.empty())
 				{
 					img = uCvMat2QImage(imageMat);
@@ -516,7 +744,6 @@ void DatabaseViewer::update(int value,
 	{
 		ULOGGER_ERROR("Slider index out of range ?");
 	}
-	UINFO("Time = %fs", timer.ticks());
 }
 
 void DatabaseViewer::sliderAMoved(int value)
@@ -544,3 +771,212 @@ void DatabaseViewer::sliderBMoved(int value)
 		ULOGGER_ERROR("Slider index out of range ?");
 	}
 }
+
+void DatabaseViewer::sliderNeighborValueChanged(int value)
+{
+	this->updateConstraintView(neighborLinks_.at(value));
+}
+
+void DatabaseViewer::sliderLoopValueChanged(int value)
+{
+	this->updateConstraintView(loopLinks_.at(value));
+}
+
+void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
+{
+	rtabmap::Transform t = link.transform();
+	ui_->label_constraint->clear();
+	if(!t.isNull() && memory_)
+	{
+		ui_->label_constraint->setText(t.prettyPrint().c_str());
+		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
+		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
+
+		float depthConstantA, depthConstantB;
+		rtabmap::Transform localTransformA, localTransformB;
+
+		std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
+		memory_->getImageDepth(link.from(), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA);
+		cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
+		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
+		cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);
+
+		std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
+		memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, depthConstantB, localTransformB);
+		cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
+		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
+		cv::Mat depth2dB = rtabmap::util3d::uncompressData(depth2dBytesB);
+
+		//cloud 3d
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+		cloudA = rtabmap::util3d::cloudFromDepthRGB(
+				imageA,
+				depthA,
+				depthA.cols/2,
+				depthA.rows/2,
+				1.0f/depthConstantA,
+				1.0f/depthConstantA,
+				1);
+
+		cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
+		cloudA = rtabmap::util3d::transformPointCloud(cloudA, localTransformA);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+		cloudB = rtabmap::util3d::cloudFromDepthRGB(
+				imageB,
+				depthB,
+				depthB.cols/2,
+				depthB.rows/2,
+				1.0f/depthConstantB,
+				1.0f/depthConstantB,
+				1);
+
+		cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
+		cloudB = rtabmap::util3d::transformPointCloud(cloudB, t*localTransformB);
+
+		//cloud 2d
+		pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+		scanA = rtabmap::util3d::depth2DToPointCloud(depth2dA);
+		scanB = rtabmap::util3d::depth2DToPointCloud(depth2dB);
+		scanB = rtabmap::util3d::transformPointCloud(scanB, t);
+
+		if(cloudA->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudA);
+		}
+		if(cloudB->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudB);
+		}
+		if(scanA->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
+		}
+		if(scanB->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
+		}
+	}
+	ui_->constraintsViewer->render();
+}
+
+void DatabaseViewer::sliderIterationsValueChanged(int value)
+{
+	if(memory_ && value >=0 && value < (int)graphes_.size())
+	{
+		if(scans_.size() == 0)
+		{
+			//update scans
+			UINFO("Update scans list...");
+			for(int i=0; i<ids_.size(); ++i)
+			{
+				std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
+				float depthConstantA;
+				rtabmap::Transform localTransformA;
+				memory_->getImageDepth(ids_.at(i), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA);
+				if(depth2dBytesA.size())
+				{
+					scans_.insert(std::make_pair(ids_.at(i), depth2dBytesA));
+				}
+			}
+			UINFO("Update scans list... done");
+		}
+
+		ui_->graphViewer->updateGraph(uValueAt(graphes_, value), links_, scans_);
+		ui_->label_iterations->setNum(value);
+	}
+}
+void DatabaseViewer::updateGraphView()
+{
+	if(ui_->dockWidget_graphView->isVisible() && poses_.size())
+	{
+		if(!uContains(poses_, ui_->spinBox_optimizationsFrom->value()))
+		{
+			QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
+						.arg(ui_->spinBox_optimizationsFrom->value())
+						.arg(poses_.begin()->first)
+						.arg(poses_.rbegin()->first));
+			return;
+		}
+
+		graphes_.clear();
+
+		std::map<int, rtabmap::Transform> finalPoses;
+		graphes_.push_back(poses_);
+
+		std::map<int, int> ids = memory_->getNeighborsId(ui_->spinBox_optimizationsFrom->value(), 0, -1, true);
+		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
+		int m = 0;
+		int toroId = 1;
+		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
+		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
+		while(ids.size())
+		{
+			for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end();)
+			{
+				if(m == iter->second)
+				{
+					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
+					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
+					++toroId;
+					ids.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+			++m;
+		}
+
+		std::map<int, rtabmap::Transform> posesToro;
+		std::multimap<int, rtabmap::Link> edgeConstraintsToro;
+		for(std::map<int, rtabmap::Transform>::iterator iter = poses_.begin(); iter!=poses_.end(); ++iter)
+		{
+			posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
+		}
+		for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin();
+			iter!=links_.end();
+			++iter)
+		{
+			edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
+		}
+
+		std::map<int, rtabmap::Transform> optimizedPosesToro;
+		rtabmap::Transform mapCorrectionToro;
+		std::list<std::map<int, rtabmap::Transform> > graphesToro;
+
+		// Optimize!
+		rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphesToro);
+
+		for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
+		{
+			std::map<int, rtabmap::Transform> tmp;
+			for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+			{
+				tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
+			}
+			graphes_.push_back(tmp);
+		}
+
+		for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
+		{
+			finalPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
+		}
+
+
+		graphes_.push_back(finalPoses);
+	}
+	if(graphes_.size())
+	{
+		ui_->horizontalSlider_iterations->setMaximum(graphes_.size()-1);
+		ui_->horizontalSlider_iterations->setValue(graphes_.size()-1);
+		ui_->dockWidget_graphView->setEnabled(true);
+		sliderIterationsValueChanged(graphes_.size()-1);
+	}
+	else
+	{
+		ui_->dockWidget_graphView->setEnabled(false);
+	}
+
+}

guilib/src/GraphViewer.cpp

@@ -0,0 +1,551 @@
+/*
+ * GraphViewer.cpp
+ *
+ *  Created on: 2014-01-24
+ *      Author: mathieu
+ */
+
+#include "GraphViewer.h"
+
+#include <QtGui/QGraphicsView>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QGraphicsScene>
+#include <QtGui/QGraphicsEllipseItem>
+#include <QtGui/QWheelEvent>
+#include <QtGui/QGraphicsSceneHoverEvent>
+#include <QtGui/QMenu>
+#include <QtGui/QDesktopServices>
+#include <QtGui/QContextMenuEvent>
+#include <QtGui/QColorDialog>
+#include <QtSvg/QSvgGenerator>
+#include <QtGui/QInputDialog>
+
+#include <QtCore/QDir>
+#include <QtCore/QDateTime>
+#include <QtCore/QUrl>
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/UCv2Qt.h>
+#include <rtabmap/utilite/UStl.h>
+
+namespace rtabmap {
+
+class NodeItem: public QGraphicsEllipseItem
+{
+public:
+	// in meter
+	NodeItem(int id, const Transform & pose, float radius, const cv::Mat & scan) :
+		QGraphicsEllipseItem(QRectF(-radius,-radius,radius*2.0f,radius*2.0f)),
+		_id(id),
+		_pose(pose),
+		_scan(scan)
+	{
+		this->setPos(-pose.y(),-pose.x());
+		this->setBrush(pen().color());
+		this->setAcceptHoverEvents(true);
+	}
+
+	void setColor(const QColor & color)
+	{
+		QPen p = this->pen();
+		p.setColor(color);
+		this->setPen(p);
+		QBrush b = this->brush();
+		b.setColor(color);
+		this->setBrush(b);
+	}
+
+	void setPose(const Transform & pose) {this->setPos(-pose.y(),-pose.x()); _pose=pose;}
+	void setScan(const cv::Mat & scan) {_scan = scan;}
+	const cv::Mat & getScan() const {return _scan;}
+
+protected:
+	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		this->setToolTip(QString("[%1] %2").arg(_id).arg(_pose.prettyPrint().c_str()));
+		this->setScale(2);
+		QGraphicsEllipseItem::hoverEnterEvent(event);
+	}
+
+	virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		qDeleteAll(this->children());
+		this->setScale(1);
+		QGraphicsEllipseItem::hoverEnterEvent(event);
+	}
+
+private:
+	int _id;
+	Transform _pose;
+	cv::Mat _scan;
+};
+
+class LinkItem: public QGraphicsLineItem
+{
+public:
+	// in meter
+	LinkItem(const Transform & poseA, const Transform & poseB, bool loopClosure) :
+		QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()),
+		_poseA(poseA),
+		_poseB(poseB),
+		_loopClosure(loopClosure)
+	{
+		this->setAcceptHoverEvents(true);
+	}
+
+	void setColor(const QColor & color)
+	{
+		QPen p = this->pen();
+		p.setColor(color);
+		this->setPen(p);
+	}
+
+	void setPoses(const Transform & poseA, const Transform & poseB)
+	{
+		this->setLine(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x());
+		_poseA = poseA;
+		_poseB = poseB;
+	}
+
+	bool isLoopClosure() const {return _loopClosure;}
+
+protected:
+	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		QGraphicsLineItem::hoverEnterEvent(event);
+	}
+
+	virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		QGraphicsLineItem::hoverEnterEvent(event);
+	}
+
+private:
+	Transform _poseA;
+	Transform _poseB;
+	bool _loopClosure;
+};
+
+GraphViewer::GraphViewer(QWidget * parent) :
+		QGraphicsView(parent),
+		_nodeColor(Qt::blue),
+		_neighborColor(Qt::blue),
+		_loopClosureColor(Qt::red),
+		_root(0),
+		_nodeRadius(0.1),
+		_linkWidth(0.15),
+		_gridMap(0)
+{
+	this->setScene(new QGraphicsScene(this));
+	this->setDragMode(QGraphicsView::ScrollHandDrag);
+	_workingDirectory = QDir::homePath();
+
+	this->scene()->clear();
+	_root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(0,0,0,0));
+
+	// add referential
+	QGraphicsLineItem * item = this->scene()->addLine(0,0,0,-1, QPen(QBrush(Qt::red), _linkWidth));
+	item->setZValue(100);
+	item->setParentItem(_root);
+	item = this->scene()->addLine(0,0,-1,0, QPen(QBrush(Qt::green), _linkWidth));
+	item->setZValue(100);
+	item->setParentItem(_root);
+
+	_gridMap = this->scene()->addPixmap(QPixmap());
+	_gridMap->scale(0.05, -0.05);
+	_gridMap->setRotation(90);
+	_gridMap->setZValue(0);
+	_gridMap->setParentItem(_root);
+}
+
+GraphViewer::~GraphViewer()
+{
+}
+
+void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
+				 const std::multimap<int, Link> & constraints,
+				 const std::map<int, std::vector<unsigned char> > & scans)
+{
+	//Hide nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+	{
+		iter.value()->hide();
+		iter.value()->setColor(_nodeColor); // reset color
+	}
+	for(QMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+	for(QMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scanClouds;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
+		if(itemIter != _nodeItems.end())
+		{
+			itemIter.value()->setPose(iter->second);
+			if(itemIter.value()->getScan().empty() && uContains(scans, iter->first))
+			{
+				cv::Mat depth2d = util3d::uncompressData(scans.at(iter->first));
+				itemIter.value()->setScan(depth2d);
+			}
+			if(!itemIter.value()->getScan().empty() && _gridMap->isVisible())
+			{
+				scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(itemIter.value()->getScan())));
+			}
+			itemIter.value()->show();
+		}
+		else
+		{
+			// create node item
+			std::map<int, std::vector<unsigned char> >::const_iterator jter = scans.find(iter->first);
+			cv::Mat depth2d;
+			if(jter != scans.end())
+			{
+				depth2d = util3d::uncompressData(jter->second);
+				if(_gridMap->isVisible())
+				{
+					scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d)));
+				}
+			}
+
+			const Transform & pose = iter->second;
+			NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius, depth2d);
+			this->scene()->addItem(item);
+			item->setZValue(2);
+			item->setColor(_nodeColor);
+			item->setParentItem(_root);
+			_nodeItems.insert(iter->first, item);
+		}
+	}
+
+	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jterA = poses.find(iter->first);
+		std::map<int, Transform>::const_iterator jterB = poses.find(iter->second.to());
+		if(jterA != poses.end() && jterB != poses.end())
+		{
+			const Transform & poseA = jterA->second;
+			const Transform & poseB = jterB->second;
+			bool loopClosure = iter->second.type() != Link::kNeighbor;
+
+			if(loopClosure && _loopLinkItems.contains(iter->first))
+			{
+				QMap<int, LinkItem*>::iterator itemIter = _loopLinkItems.find(iter->first);
+				itemIter.value()->setPoses(poseA, poseB);
+				itemIter.value()->show();
+			}
+			else if(!loopClosure && _neighborLinkItems.contains(iter->first))
+			{
+				QMap<int, LinkItem*>::iterator itemIter = _neighborLinkItems.find(iter->first);
+				itemIter.value()->setPoses(poseA, poseB);
+				itemIter.value()->show();
+			}
+			else
+			{
+				//create a link item
+				LinkItem * item = new LinkItem(poseA, poseB, loopClosure);
+				QPen p = item->pen();
+				p.setWidthF(_linkWidth);
+				item->setPen(p);
+				item->setColor(loopClosure?_loopClosureColor:_neighborColor);
+				this->scene()->addItem(item);
+				item->setZValue(1);
+				item->setParentItem(_root);
+				if(loopClosure)
+				{
+					_loopLinkItems.insert(iter->first, item);
+				}
+				else
+				{
+					_neighborLinkItems.insert(iter->first, item);
+				}
+			}
+		}
+	}
+
+	//remove not used nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _nodeItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+	for(QMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _loopLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+	for(QMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _neighborLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	if(scanClouds.size())
+	{
+		float xMin=0.0f, yMin=0.0f;
+		cv::Mat map8S = util3d::create2DMap(poses, scanClouds, 0.05, xMin, yMin);
+		cv::Mat map8U(map8S.rows, map8S.cols, CV_8U);
+		//convert to gray scaled map
+		for (int i = 0; i < map8S.rows; ++i)
+		{
+			for (int j = 0; j < map8S.cols; ++j)
+			{
+				char gray = map8S.at<char>(i, j);
+				if(gray == -1)
+				{
+					gray = 89;
+				}
+				else if(gray == 0)
+				{
+					gray = 178;
+				}
+				else if(gray == 100)
+				{
+					gray = 0;
+				}
+				map8U.at<char>(i, j) = gray;
+			}
+		}
+		QImage image = uCvMat2QImage(map8U, false);
+		_gridMap->setPixmap(QPixmap::fromImage(image));
+		_gridMap->setPos(-yMin, -xMin);
+	}
+
+	if(_nodeItems.size())
+	{
+		(--_nodeItems.end()).value()->setColor(Qt::gray);
+	}
+
+	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+	this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+}
+
+void GraphViewer::clearGraph()
+{
+	qDeleteAll(_nodeItems);
+	_nodeItems.clear();
+	qDeleteAll(_neighborLinkItems);
+	_neighborLinkItems.clear();
+	qDeleteAll(_loopLinkItems);
+	_loopLinkItems.clear();
+	_gridMap->setPixmap(QPixmap());
+	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+}
+
+void GraphViewer::wheelEvent ( QWheelEvent * event )
+{
+	if(event->delta() > 0)
+	{
+		this->scale(0.95, 0.95);
+	}
+	else
+	{
+		this->scale(1.05, 1.05);
+	}
+}
+
+void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
+{
+	QMenu menu;
+	QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
+	QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
+	menu.addSeparator();
+	QAction * aChangeNodeColor = menu.addAction(tr("Set node color..."));
+	QAction * aChangeNeighborColor = menu.addAction(tr("Set neighbor link color..."));
+	QAction * aChangeLoopColor = menu.addAction(tr("Set loop closure link color..."));
+	menu.addSeparator();
+	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
+	QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
+	menu.addSeparator();
+	QAction * aShowHideGridMap;
+	if(_gridMap->isVisible())
+	{
+		aShowHideGridMap = menu.addAction(tr("Hide grid map"));
+	}
+	else
+	{
+		aShowHideGridMap = menu.addAction(tr("Show grid map"));
+	}
+
+	QAction * r = menu.exec(event->globalPos());
+	if(r == aScreenShotPNG || r == aScreenShotSVG)
+	{
+		if(_root)
+		{
+			QString targetDir = _workingDirectory + "/ScreensCaptured";
+			QDir dir;
+			if(!dir.exists(targetDir))
+			{
+				dir.mkdir(targetDir);
+			}
+			targetDir += "/";
+			targetDir += "Graph_view";
+			if(!dir.exists(targetDir))
+			{
+				dir.mkdir(targetDir);
+			}
+			targetDir += "/";
+			bool isPNG = r == aScreenShotPNG;
+			QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (isPNG?".png":".svg"));
+
+			//_root->setScale(this->transform().m11()); // current view
+			_root->setScale(20); // grid map precision (5cm)
+
+			this->scene()->clearSelection();                                  // Selections would also render to the file
+			this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+			QSize sceneSize = this->scene()->sceneRect().size().toSize();
+
+			if(isPNG)
+			{
+				QImage image(sceneSize, QImage::Format_ARGB32);  // Create the image with the exact size of the shrunk scene
+				image.fill(Qt::transparent);                     // Start all pixels transparent
+				QPainter painter(&image);
+
+				this->scene()->render(&painter);
+				image.save(targetDir + name);
+			}
+			else
+			{
+				QSvgGenerator svgGen;
+
+				svgGen.setFileName( targetDir + name );
+				svgGen.setSize(sceneSize);
+				svgGen.setViewBox(QRect(0, 0, sceneSize.width(), sceneSize.height()));
+				svgGen.setTitle(tr("RTAB-Map graph"));
+				svgGen.setDescription(tr("RTAB-Map map and graph"));
+
+				QPainter painter( &svgGen );
+
+				this->scene()->render(&painter);
+			}
+
+			//reset scale
+			_root->setScale(1.0f);
+			this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+
+
+			QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
+		}
+	}
+	else if(r == aChangeNodeColor ||
+			r == aChangeNeighborColor ||
+			r == aChangeLoopColor)
+	{
+		QColor color;
+		if(r == aChangeNodeColor)
+		{
+			color = _nodeColor;
+		}
+		else if(r == aChangeNeighborColor)
+		{
+			color = _neighborColor;
+		}
+		else if(r == aChangeLoopColor)
+		{
+			color = _loopClosureColor;
+		}
+		color = QColorDialog::getColor(color, this);
+
+		if(r == aChangeNodeColor)
+		{
+			_nodeColor = color;
+		}
+		else if(r == aChangeNeighborColor)
+		{
+			_neighborColor = color;
+		}
+		else if(r == aChangeLoopColor)
+		{
+			_loopClosureColor = color;
+		}
+
+		QList<QGraphicsItem*> items = this->scene()->items();
+		for(int i=0; i<items.size(); ++i)
+		{
+			LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+			NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
+			if(r == aChangeNodeColor && node)
+			{
+				node->setColor(color);
+			}
+			else if(r == aChangeNeighborColor && link && !link->isLoopClosure())
+			{
+				link->setColor(color);
+			}
+			else if(r == aChangeLoopColor && link && link->isLoopClosure())
+			{
+				link->setColor(color);
+			}
+		}
+	}
+	else if(r == aSetNodeSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 1, 2, &ok);
+		if(ok)
+		{
+			_nodeRadius = value;
+			QList<QGraphicsItem*> items = this->scene()->items();
+			for(int i=0; i<items.size(); ++i)
+			{
+				NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
+				if(node)
+				{
+					node->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
+				}
+			}
+		}
+	}
+	else if(r == aSetLinkSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 1, 2, &ok);
+		if(ok)
+		{
+			_linkWidth = value;
+			QList<QGraphicsItem*> items = this->scene()->items();
+			for(int i=0; i<items.size(); ++i)
+			{
+				QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
+				if(line)
+				{
+					QPen pen = line->pen();
+					pen.setWidthF(_linkWidth);
+					line->setPen(pen);
+				}
+			}
+		}
+	}
+	else if(r == aShowHideGridMap)
+	{
+		_gridMap->setVisible(!_gridMap->isVisible());
+	}
+}
+
+} /* namespace rtabmap */

guilib/src/GraphViewer.h

@@ -0,0 +1,55 @@
+/*
+ * GraphViewer.h
+ *
+ *  Created on: 2014-01-24
+ *      Author: mathieu
+ */
+
+#ifndef GRAPHVIEWER_H_
+#define GRAPHVIEWER_H_
+
+#include <QtGui/QGraphicsView>
+#include <QtCore/QMap>
+#include <rtabmap/core/Link.h>
+#include <map>
+
+class QGraphicsItem;
+class QGraphicsPixmapItem;
+
+namespace rtabmap {
+
+class NodeItem;
+class LinkItem;
+
+class GraphViewer : public QGraphicsView {
+public:
+	GraphViewer(QWidget * parent = 0);
+	virtual ~GraphViewer();
+
+	void updateGraph(const std::map<int, Transform> & poses,
+					 const std::multimap<int, Link> & constraints,
+					 const std::map<int, std::vector<unsigned char> > & scans);
+	void clearGraph();
+
+	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
+
+protected:
+	virtual void wheelEvent ( QWheelEvent * event );
+	virtual void contextMenuEvent(QContextMenuEvent * event);
+
+private:
+	QString _workingDirectory;
+	QColor _nodeColor;
+	QColor _neighborColor;
+	QColor _loopClosureColor;
+	QGraphicsItem * _root;
+	QMap<int, NodeItem*> _nodeItems;
+	QMap<int, LinkItem*> _neighborLinkItems;
+	QMap<int, LinkItem*> _loopLinkItems;
+	float _nodeRadius;
+	float _linkWidth;
+	QGraphicsPixmapItem * _gridMap;
+};
+
+} /* namespace rtabmap */
+#endif /* GRAPHVIEWER_H_ */

guilib/src/MainWindow.cpp

@@ -109,7 +109,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_posteriorCurve(0),
 	_likelihoodCurve(0),
 	_rawLikelihoodCurve(0),
-	_autoScreenCaptureFormat("png")
+	_autoScreenCaptureOdomSync(false)
 {
 	ULOGGER_DEBUG("");
 
@@ -152,9 +152,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 		_ui->dockWidget_console->setVisible(false);
 		_ui->dockWidget_loopClosureViewer->setVisible(false);
 		_ui->dockWidget_mapVisibility->setVisible(false);
+		_ui->dockWidget_graphViewer->setVisible(false);
 		//_ui->dockWidget_cloudViewer->setVisible(false);
+		//_ui->dockWidget_imageView->setVisible(false);
 	}
 
+	_ui->widget_mainWindow->setVisible(false);
+
 	if(prefDialog)
 	{
 		_preferencesDialog = prefDialog;
@@ -218,6 +222,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
 
 	// Dock Widget view actions (Menu->Window)
+	_ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_likelihood->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_rawlikelihood->toggleViewAction());
@@ -226,6 +231,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
+	_ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
 	_ui->toolBar->setWindowTitle(tr("Control toolbar"));
 	QAction * a = _ui->menuShow_view->addAction("Progress dialog");
@@ -246,8 +252,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap()));
 	connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
+	connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
 	connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
 	connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
+	connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot()));
 	connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9()));
 	connect(_ui->action16_10, SIGNAL(triggered()), this, SLOT(setAspectRatio16_10()));
 	connect(_ui->action4_3, SIGNAL(triggered()), this, SLOT(setAspectRatio4_3()));
@@ -334,6 +342,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	this->applyPrefSettings(PreferencesDialog::kPanelAll);
 
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
+	_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 
 	splash.close();
 }
@@ -353,11 +362,11 @@ void MainWindow::setupMainLayout(bool vertical)
 {
 	if(vertical)
 	{
-		qobject_cast<QHBoxLayout *>(_ui->centralwidget->layout())->setDirection(QBoxLayout::TopToBottom);
+		qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
 	}
 	else if(!vertical)
 	{
-		qobject_cast<QHBoxLayout *>(_ui->centralwidget->layout())->setDirection(QBoxLayout::LeftToRight);
+		qobject_cast<QHBoxLayout *>(_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
 	}
 }
 
@@ -389,6 +398,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
 		//write settings before quit?
 		_preferencesDialog->saveMainWindowState(this);
 
+		_ui->dockWidget_imageView->close();
 		_ui->dockWidget_likelihood->close();
 		_ui->dockWidget_rawlikelihood->close();
 		_ui->dockWidget_posterior->close();
@@ -397,6 +407,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
 		_ui->dockWidget_cloudViewer->close();
 		_ui->dockWidget_loopClosureViewer->close();
 		_ui->dockWidget_mapVisibility->close();
+		_ui->dockWidget_graphViewer->close();
 
 		if(_camera)
 		{
@@ -610,6 +621,11 @@ void MainWindow::processOdometry(const rtabmap::Image & data)
 	_ui->widget_cloudViewer->render();
 
 	_lastOdometryProcessed = true;
+
+	if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
+	{
+		this->captureScreen();
+	}
 }
 
 void MainWindow::processStats(const rtabmap::Statistics & stat)
@@ -686,9 +702,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				iter != stat.getDepth2ds().end();
 				++iter)
 			{
-				if(!iter->second.empty() && !_depths2DMap.contains(iter->first))
+				if(!iter->second.empty())
 				{
-					_depths2DMap.insert(iter->first, iter->second);
+					_depths2DMap.insert(std::make_pair(iter->first, iter->second));
 				}
 			}
 		}
@@ -923,7 +939,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			// update pose only if a odometry is not received
 			updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose());
+
+			// update some widgets
 			_ui->widget_mapVisibility->setMap(stat.poses());
+			if(_ui->graphicsView_graphView->isVisible())
+			{
+				_ui->graphicsView_graphView->updateGraph(stat.poses(), stat.constraints(), _depths2DMap);
+			}
+
 			_odometryReceived = false;
 
 			_odometryCorrection = stat.mapCorrection();
@@ -951,7 +974,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						std::multimap<int, cv::KeyPoint>(),
 						std::multimap<int, pcl::PointXYZ>(),
 						Transform(),
-						_depths2DMap.value(loopOldId, std::vector<unsigned char>()),
+						uValue(_depths2DMap, loopOldId, std::vector<unsigned char>()),
 						_imagesMap.value(loopOldId, std::vector<unsigned char>()),
 						_depthsMap.value(loopOldId, std::vector<unsigned char>()),
 						_depthConstantsMap.value(loopOldId, 0.0f),
@@ -963,7 +986,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						std::multimap<int, cv::KeyPoint>(),
 						std::multimap<int, pcl::PointXYZ>(),
 						loopClosureTransform,
-						_depths2DMap.value(loopNewId, std::vector<unsigned char>()),
+						uValue(_depths2DMap, loopNewId, std::vector<unsigned char>()),
 						_imagesMap.value(loopNewId, std::vector<unsigned char>()),
 						_depthsMap.value(loopNewId, std::vector<unsigned char>()),
 						_depthConstantsMap.value(loopNewId, 0.0f),
@@ -991,7 +1014,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	float elapsedTime = static_cast<float>(totalTime.elapsed());
 	UINFO("Processing statistics time = %fs", elapsedTime/1000.0f);
 	_ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
-	this->captureScreen();
+	if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
+	{
+		this->captureScreen();
+	}
 	_processingStatistics = false;
 }
 
@@ -1105,10 +1131,10 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Tr
 					_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 					_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
-				else if(_depths2DMap.contains(iter->first))
+				else if(uContains(_depths2DMap, iter->first))
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(iter->first));
+					cv::Mat depth2d = util3d::uncompressData(_depths2DMap.at(iter->first));
 					cloud = util3d::depth2DToPointCloud(depth2d);
 					if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, iter->second))
 					{
@@ -1251,6 +1277,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		UINFO(" depthConstants = %d", event.getDepthConstants().size());
 		UINFO(" localTransforms = %d", event.getLocalTransforms().size());
 		UINFO(" poses = %d", event.getPoses().size());
+		UINFO(" constraints = %d", event.getConstraints().size());
 
 		_initProgressDialog->appendText("Inserting data in the cache...");
 
@@ -1285,7 +1312,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			iter!=event.getDepths2d().end();
 			++iter)
 		{
-			_depths2DMap.insert(iter->first, iter->second);
+			_depths2DMap.insert(std::make_pair(iter->first, iter->second));
 		}
 		_initProgressDialog->appendText(tr("Inserted %1 laser scans.").arg(_depths2DMap.size()));
 		_initProgressDialog->incrementStep();
@@ -1307,8 +1334,20 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		{
 			_initProgressDialog->appendText("Updating the 3D map cloud...");
 			_initProgressDialog->incrementStep();
+			QApplication::processEvents();
 			this->updateMapCloud(event.getPoses(), Transform());
 			_initProgressDialog->appendText("Updating the 3D map cloud... done.");
+
+			if(_ui->graphicsView_graphView->isVisible())
+			{
+				_initProgressDialog->appendText("Updating the graph view...");
+				_initProgressDialog->incrementStep();
+				_ui->graphicsView_graphView->updateGraph(
+						event.getPoses(),
+						event.getConstraints(),
+						event.getDepths2d().size()?event.getDepths2d():_depths2DMap);
+				_initProgressDialog->appendText("Updating the graph view... done.");
+			}
 		}
 		else
 		{
@@ -1336,6 +1375,8 @@ void MainWindow::applyAllPrefSettings()
 		}
 	}
 
+	_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
+
 	this->applyPrefSettings(PreferencesDialog::kPanelAll);
 	this->applyPrefSettings(_preferencesDialog->getAllParameters());
 }
@@ -1684,10 +1725,6 @@ void MainWindow::changeMappingMode()
 
 void MainWindow::captureScreen()
 {
-	if(!_ui->actionAuto_screen_capture->isChecked())
-	{
-		return;
-	}
 	QString targetDir = _preferencesDialog->getWorkingDirectory() + "/ScreensCaptured";
 	QDir dir;
 	if(!dir.exists(targetDir))
@@ -1701,11 +1738,13 @@ void MainWindow::captureScreen()
 		dir.mkdir(targetDir);
 	}
 	targetDir += "/";
-	QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".") + _autoScreenCaptureFormat;
+	QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + ".png");
 	_ui->statusbar->clearMessage();
 	QPixmap figure = QPixmap::grabWidget(this);
 	figure.save(targetDir + name);
-	_ui->statusbar->showMessage(tr("Screen captured \"%1\"").arg(targetDir + name), _preferencesDialog->getTimeLimit()*500);
+	QString msg = tr("Screen captured \"%1\"").arg(targetDir + name);
+	_ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
+	_ui->widget_console->appendMsg(msg);
 }
 
 void MainWindow::beep()
@@ -2408,6 +2447,65 @@ void MainWindow::downloadAllClouds()
 	}
 }
 
+void MainWindow::downloadPoseGraph()
+{
+	bool showAll = _state != kMonitoringPaused && _state != kMonitoring;
+
+	QStringList items;
+	items.append("Current map optimized");
+	if(showAll)
+	{
+		items.append("Current map not optimized");
+	}
+	items.append("Full map optimized");
+	if(showAll)
+	{
+		items.append("Full map not optimized");
+	}
+	bool ok;
+	QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, showAll?2:1, false, &ok);
+	if(ok)
+	{
+		bool optimized=false, full=false;
+		if(item.compare("Current map optimized") == 0)
+		{
+			optimized = true;
+		}
+		else if(item.compare("Current map not optimized") == 0)
+		{
+
+		}
+		else if(item.compare("Full map optimized") == 0)
+		{
+			full=true;
+			optimized=true;
+		}
+		else if(item.compare("Full map not optimized") == 0)
+		{
+			full=true;
+		}
+		else
+		{
+			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
+		}
+
+		UINFO("Download the graph...");
+		_initProgressDialog->setAutoClose(true, 1);
+		_initProgressDialog->resetProgress();
+		_initProgressDialog->show();
+		_initProgressDialog->appendText(tr("Downloading the graph (full=%1 ,optimized=%2)...")
+				.arg(full?"true":"false").arg(optimized?"true":"false"));
+		if(full)
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishGraphFull, optimized?1:0));
+		}
+		else
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishGraph, optimized?1:0));
+		}
+	}
+}
+
 void MainWindow::clearTheCache()
 {
 	_imagesMap.clear();
@@ -2435,6 +2533,7 @@ void MainWindow::clearTheCache()
 	_ui->label_stats_loopClosuresRejected->setText("0");
 	_refIds.clear();
 	_loopClosureIds.clear();
+	_ui->graphicsView_graphView->clearGraph();
 }
 
 void MainWindow::updateElapsedTime()
@@ -2520,17 +2619,28 @@ void MainWindow::selectScreenCaptureFormat(bool checked)
 	if(checked)
 	{
 		QStringList items;
-		items << QString("png") << QString("jpg");
+		items << QString("Synchronize with map update") << QString("Synchronize with odometry update");
 		bool ok;
-		QString item = QInputDialog::getItem(this, tr("Select format"), tr("Format:"), items, 0, false, &ok);
+		QString item = QInputDialog::getItem(this, tr("Select synchronization behavior"), tr("Sync:"), items, 0, false, &ok);
 		if(ok && !item.isEmpty())
 		{
-			_autoScreenCaptureFormat = item;
+			if(item.compare("Synchronize with map update") == 0)
+			{
+				_autoScreenCaptureOdomSync = false;
+			}
+			else
+			{
+				_autoScreenCaptureOdomSync = true;
+			}
 		}
-		this->captureScreen();
 	}
 }
 
+void MainWindow::takeScreenshot()
+{
+	this->captureScreen();
+}
+
 void MainWindow::setAspectRatio(int w, int h)
 {
 	QRect rect = this->geometry();
@@ -2609,7 +2719,7 @@ void MainWindow::savePointClouds()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
-		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1);
 
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
 		if(button == QMessageBox::No)
@@ -2637,7 +2747,7 @@ void MainWindow::saveMeshes()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
-		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1);
 
 		std::map<int, pcl::PolygonMesh::Ptr> meshes;
 		if(button == QMessageBox::No)
@@ -3062,6 +3172,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
 {
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	int i=0;
+	int count = 0;
 	for(std::map<int, Transform>::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
 	{
 		bool inserted = false;
@@ -3089,7 +3200,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
 				if(cloud->size())
 				{
 					*assembledCloud += *cloud;
+
 					inserted = true;
+					++count;
 				}
 			}
 			else
@@ -3105,6 +3218,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
 		if(inserted)
 		{
 			_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
+
+			if(count % 100 == 0)
+			{
+				if(assembledCloud->size() && _preferencesDialog->getCloudVoxelSize(2))
+				{
+					assembledCloud = util3d::voxelize(assembledCloud, _preferencesDialog->getCloudVoxelSize(2));
+				}
+			}
 		}
 		else
 		{
@@ -3116,13 +3237,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
 
 	if(assembledCloud->size() && _preferencesDialog->getCloudVoxelSize(2))
 	{
-		_initProgressDialog->appendText(tr("Voxelize the assembled cloud (%1 points)...").arg(assembledCloud->size()));
-		_initProgressDialog->incrementStep();
-		_initProgressDialog->appendText(tr("Voxelize the assembled cloud (%1 points)... done.").arg(assembledCloud->size()));
-
-		UDEBUG("Voxelize the assembled cloud of %d points (%f m)", (int)assembledCloud->size(), _preferencesDialog->getCloudVoxelSize(2));
 		assembledCloud = util3d::voxelize(assembledCloud, _preferencesDialog->getCloudVoxelSize(2));
 	}
+
 	return assembledCloud;
 }
 

guilib/src/PreferencesDialog.cpp

@@ -374,6 +374,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str());
+	_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
@@ -2236,13 +2237,12 @@ void PreferencesDialog::updateKpROI()
 
 void PreferencesDialog::changeDatabasePath()
 {
-	QString path = QFileDialog::getSaveFileName(
+	QString path = QFileDialog::getOpenFileName(
 			this,
 			tr("Select database file..."),
 			_ui->lineEdit_databasePath->text(),
 			tr("RTAB-Map database files (*.db)"),
-			0,
-			QFileDialog::DontConfirmOverwrite);
+			0);
 
 	if(!path.isEmpty())
 	{

guilib/src/ui/DatabaseViewer.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>736</width>
-    <height>456</height>
+    <width>1041</width>
+    <height>637</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -287,7 +287,7 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>736</width>
+     <width>1041</width>
      <height>25</height>
     </rect>
    </property>
@@ -298,6 +298,7 @@
     <addaction name="actionOpen_database"/>
     <addaction name="separator"/>
     <addaction name="actionExport"/>
+    <addaction name="actionExtract_images"/>
     <addaction name="separator"/>
     <addaction name="actionQuit"/>
    </widget>
@@ -311,10 +312,167 @@
     <addaction name="actionClean_database"/>
     <addaction name="actionClean_local_graph"/>
     <addaction name="separator"/>
+    <addaction name="actionView_3D_map"/>
     <addaction name="actionGenerate_3D_map_pcd"/>
    </widget>
+   <widget class="QMenu" name="menuView">
+    <property name="title">
+     <string>View</string>
+    </property>
+   </widget>
    <addaction name="menuFile"/>
    <addaction name="menuEdit"/>
+   <addaction name="menuView"/>
+  </widget>
+  <widget class="QDockWidget" name="dockWidget_constraints">
+   <property name="windowTitle">
+    <string>Constraints view</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>2</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents">
+    <layout class="QVBoxLayout" name="verticalLayout_8" stretch="1,0,0">
+     <item>
+      <widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_constraint">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <layout class="QFormLayout" name="formLayout">
+       <item row="0" column="0">
+        <widget class="QLabel" name="label">
+         <property name="text">
+          <string>Neighbor links</string>
+         </property>
+        </widget>
+       </item>
+       <item row="0" column="1">
+        <widget class="QSlider" name="horizontalSlider_neighbors">
+         <property name="orientation">
+          <enum>Qt::Horizontal</enum>
+         </property>
+         <property name="tickPosition">
+          <enum>QSlider::TicksAbove</enum>
+         </property>
+        </widget>
+       </item>
+       <item row="1" column="0">
+        <widget class="QLabel" name="label_6">
+         <property name="text">
+          <string>Loop closure links</string>
+         </property>
+        </widget>
+       </item>
+       <item row="1" column="1">
+        <widget class="QSlider" name="horizontalSlider_loops">
+         <property name="orientation">
+          <enum>Qt::Horizontal</enum>
+         </property>
+         <property name="tickPosition">
+          <enum>QSlider::TicksAbove</enum>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
+    </layout>
+   </widget>
+  </widget>
+  <widget class="QDockWidget" name="dockWidget_graphView">
+   <property name="windowTitle">
+    <string>Graph view</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>1</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_2">
+    <layout class="QVBoxLayout" name="verticalLayout_9">
+     <item>
+      <widget class="rtabmap::GraphViewer" name="graphViewer"/>
+     </item>
+     <item>
+      <layout class="QHBoxLayout" name="horizontalLayout_4">
+       <item>
+        <widget class="QLabel" name="label_iterations">
+         <property name="text">
+          <string>#</string>
+         </property>
+        </widget>
+       </item>
+       <item>
+        <widget class="QSlider" name="horizontalSlider_iterations">
+         <property name="orientation">
+          <enum>Qt::Horizontal</enum>
+         </property>
+         <property name="tickPosition">
+          <enum>QSlider::TicksAbove</enum>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
+     <item>
+      <layout class="QFormLayout" name="formLayout_2">
+       <item row="0" column="0">
+        <widget class="QLabel" name="label_7">
+         <property name="text">
+          <string>TORO iterations:</string>
+         </property>
+        </widget>
+       </item>
+       <item row="0" column="1">
+        <widget class="QSpinBox" name="spinBox_iterations">
+         <property name="suffix">
+          <string/>
+         </property>
+         <property name="minimum">
+          <number>10</number>
+         </property>
+         <property name="maximum">
+          <number>1000</number>
+         </property>
+         <property name="value">
+          <number>100</number>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="0">
+        <widget class="QLabel" name="label_8">
+         <property name="text">
+          <string>Optimize from:</string>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="1">
+        <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
+       </item>
+       <item row="1" column="1">
+        <widget class="QCheckBox" name="checkBox_initGuess">
+         <property name="text">
+          <string/>
+         </property>
+         <property name="checked">
+          <bool>true</bool>
+         </property>
+        </widget>
+       </item>
+       <item row="1" column="0">
+        <widget class="QLabel" name="label_9">
+         <property name="text">
+          <string>Initial tree guess:</string>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
+    </layout>
+   </widget>
   </widget>
   <action name="actionOpen_database">
    <property name="text">
@@ -358,7 +516,7 @@
   </action>
   <action name="actionGenerate_3D_map_pcd">
    <property name="text">
-    <string>Generate 3D map (.pcd) ...</string>
+    <string>Export 3D map (.pcd) ...</string>
    </property>
   </action>
   <action name="actionExport">
@@ -366,6 +524,16 @@
     <string>Export...</string>
    </property>
   </action>
+  <action name="actionExtract_images">
+   <property name="text">
+    <string>Extract images...</string>
+   </property>
+  </action>
+  <action name="actionView_3D_map">
+   <property name="text">
+    <string>View 3D map...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>
@@ -373,6 +541,17 @@
    <extends>QGraphicsView</extends>
    <header>rtabmap/gui/ImageView.h</header>
   </customwidget>
+  <customwidget>
+   <class>rtabmap::CloudViewer</class>
+   <extends>QWidget</extends>
+   <header>rtabmap/gui/CloudViewer.h</header>
+   <container>1</container>
+  </customwidget>
+  <customwidget>
+   <class>rtabmap::GraphViewer</class>
+   <extends>QGraphicsView</extends>
+   <header>GraphViewer.h</header>
+  </customwidget>
  </customwidgets>
  <resources/>
  <connections/>

guilib/src/ui/mainWindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>809</width>
-    <height>661</height>
+    <width>947</width>
+    <height>741</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -20,88 +20,13 @@
   <property name="dockOptions">
    <set>QMainWindow::AllowNestedDocks|QMainWindow::AllowTabbedDocks|QMainWindow::AnimatedDocks|QMainWindow::VerticalTabs</set>
   </property>
-  <widget class="QWidget" name="centralwidget">
-   <layout class="QHBoxLayout" name="horizontalLayout_3">
-    <property name="spacing">
-     <number>0</number>
-    </property>
-    <property name="margin">
-     <number>0</number>
-    </property>
-    <item>
-     <layout class="QVBoxLayout" name="verticalLayout_2">
-      <property name="spacing">
-       <number>0</number>
-      </property>
-      <item>
-       <widget class="QLabel" name="label_refId">
-        <property name="minimumSize">
-         <size>
-          <width>0</width>
-          <height>18</height>
-         </size>
-        </property>
-        <property name="text">
-         <string/>
-        </property>
-        <property name="alignment">
-         <set>Qt::AlignCenter</set>
-        </property>
-       </widget>
-      </item>
-      <item>
-       <widget class="rtabmap::ImageView" name="imageView_source">
-        <property name="verticalScrollBarPolicy">
-         <enum>Qt::ScrollBarAlwaysOff</enum>
-        </property>
-        <property name="horizontalScrollBarPolicy">
-         <enum>Qt::ScrollBarAlwaysOff</enum>
-        </property>
-       </widget>
-      </item>
-     </layout>
-    </item>
-    <item>
-     <layout class="QVBoxLayout" name="verticalLayout">
-      <property name="spacing">
-       <number>0</number>
-      </property>
-      <item>
-       <widget class="QLabel" name="label_matchId">
-        <property name="minimumSize">
-         <size>
-          <width>0</width>
-          <height>18</height>
-         </size>
-        </property>
-        <property name="text">
-         <string/>
-        </property>
-        <property name="alignment">
-         <set>Qt::AlignCenter</set>
-        </property>
-       </widget>
-      </item>
-      <item>
-       <widget class="rtabmap::ImageView" name="imageView_loopClosure">
-        <property name="verticalScrollBarPolicy">
-         <enum>Qt::ScrollBarAlwaysOff</enum>
-        </property>
-        <property name="horizontalScrollBarPolicy">
-         <enum>Qt::ScrollBarAlwaysOff</enum>
-        </property>
-       </widget>
-      </item>
-     </layout>
-    </item>
-   </layout>
-  </widget>
+  <widget class="QWidget" name="widget_mainWindow"/>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>809</width>
+     <width>947</width>
      <height>25</height>
     </rect>
    </property>
@@ -116,7 +41,9 @@
      <string>Edit</string>
     </property>
     <addaction name="actionApply_settings_to_the_detector"/>
+    <addaction name="separator"/>
     <addaction name="actionDownload_all_clouds"/>
+    <addaction name="actionDownload_graph"/>
     <addaction name="separator"/>
     <addaction name="actionOpen_working_directory"/>
     <addaction name="actionPrint_loop_closure_IDs_to_console"/>
@@ -212,6 +139,7 @@
     </widget>
     <addaction name="menuShow_view"/>
     <addaction name="menuFigures"/>
+    <addaction name="actionScreenshot"/>
     <addaction name="actionAuto_screen_capture"/>
     <addaction name="menuAspect_ratio_2"/>
     <addaction name="separator"/>
@@ -642,6 +570,111 @@
     </layout>
    </widget>
   </widget>
+  <widget class="QDockWidget" name="dockWidget_graphViewer">
+   <property name="windowTitle">
+    <string>Graph view</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>8</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_9">
+    <layout class="QVBoxLayout" name="verticalLayout_10">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="rtabmap::GraphViewer" name="graphicsView_graphView"/>
+     </item>
+    </layout>
+   </widget>
+  </widget>
+  <widget class="QDockWidget" name="dockWidget_imageView">
+   <property name="windowTitle">
+    <string>Loop closure detection</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>2</number>
+   </attribute>
+   <widget class="QWidget" name="layout_imageview">
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <layout class="QVBoxLayout" name="verticalLayout_2">
+       <property name="spacing">
+        <number>0</number>
+       </property>
+       <item>
+        <widget class="QLabel" name="label_refId">
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>18</height>
+          </size>
+         </property>
+         <property name="text">
+          <string/>
+         </property>
+         <property name="alignment">
+          <set>Qt::AlignCenter</set>
+         </property>
+        </widget>
+       </item>
+       <item>
+        <widget class="rtabmap::ImageView" name="imageView_source">
+         <property name="verticalScrollBarPolicy">
+          <enum>Qt::ScrollBarAlwaysOff</enum>
+         </property>
+         <property name="horizontalScrollBarPolicy">
+          <enum>Qt::ScrollBarAlwaysOff</enum>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
+     <item>
+      <layout class="QVBoxLayout" name="verticalLayout">
+       <property name="spacing">
+        <number>0</number>
+       </property>
+       <item>
+        <widget class="QLabel" name="label_matchId">
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>18</height>
+          </size>
+         </property>
+         <property name="text">
+          <string/>
+         </property>
+         <property name="alignment">
+          <set>Qt::AlignCenter</set>
+         </property>
+        </widget>
+       </item>
+       <item>
+        <widget class="rtabmap::ImageView" name="imageView_loopClosure">
+         <property name="verticalScrollBarPolicy">
+          <enum>Qt::ScrollBarAlwaysOff</enum>
+         </property>
+         <property name="horizontalScrollBarPolicy">
+          <enum>Qt::ScrollBarAlwaysOff</enum>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
+    </layout>
+   </widget>
+  </widget>
   <action name="actionExit">
    <property name="text">
     <string>Exit</string>
@@ -936,6 +969,16 @@
     <string>Generate TORO graph (*.graph)...</string>
    </property>
   </action>
+  <action name="actionDownload_graph">
+   <property name="text">
+    <string>Download graph only</string>
+   </property>
+  </action>
+  <action name="actionScreenshot">
+   <property name="text">
+    <string>Take a screenshot</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>
@@ -979,6 +1022,11 @@
    <header>MapVisibilityWidget.h</header>
    <container>1</container>
   </customwidget>
+  <customwidget>
+   <class>rtabmap::GraphViewer</class>
+   <extends>QGraphicsView</extends>
+   <header>GraphViewer.h</header>
+  </customwidget>
  </customwidgets>
  <resources>
   <include location="../GuiLib.qrc"/>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>777</width>
-            <height>826</height>
+            <width>761</width>
+            <height>867</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>12</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3713,8 +3713,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </layout>
                        </item>
                        <item>
-                        <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,1">
-                         <item>
+                        <layout class="QFormLayout" name="formLayout_5">
+                         <item row="0" column="0">
                           <widget class="QComboBox" name="globalDetection_icpType">
                            <property name="sizeAdjustPolicy">
                             <enum>QComboBox::AdjustToContents</enum>
@@ -3736,13 +3736,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </item>
                           </widget>
                          </item>
-                         <item>
+                         <item row="0" column="1">
                           <widget class="QLabel" name="label_19">
                            <property name="text">
                             <string>Use ICP estimation. If ICP 2D is selected, laser scans are required.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_31">
+                           <property name="text">
+                            <string>TORO graph optimization iterations</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="0">
+                          <widget class="QSpinBox" name="globalDetection_toroIterations">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>10000</number>
+                           </property>
+                           <property name="value">
+                            <number>100</number>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>