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
2026-09-03 01:50:24 +08:00 · 2014-02-11 23:04:22 +00:00
parent df3d694829
commit ea570abb8c
27 changed files with 2178 additions and 242 deletions
--- a/guilib/src/DatabaseViewer.cpp
+++ b/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);
+	}
+
+}