DbViewer: added menu actions to view/update/export optimized mesh saved in database. CloudViewer: cubes can be added for convenience, fixed double-click not always working. util3d: added conversion function from LaserScan to PointCloud2, added conversion functions between polygons format saved in database and PCL polygons format with vertices.

corelib/include/rtabmap/core/util3d.h

@@ -228,6 +228,7 @@ cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXY
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 
+pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
 // For 2d laserScan, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
 // For laserScan without normals, normals are set to null.

corelib/include/rtabmap/core/util3d_surface.h

@@ -175,6 +175,15 @@ pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
 void RTABMAP_EXP concatenateTextureMaterials(
 		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
 
+std::vector<std::vector<unsigned int> > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<pcl::Vertices> & polygons);
+std::vector<std::vector<std::vector<unsigned int> > > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<std::vector<pcl::Vertices> > & polygons);
+std::vector<pcl::Vertices> RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<unsigned int> > & polygons);
+std::vector<std::vector<pcl::Vertices> > RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons);
+
 pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,

corelib/src/util3d.cpp

@@ -2285,6 +2285,45 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
+pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
+{
+	pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+	if(laserScan.isEmpty())
+	{
+		return cloud;
+	}
+
+	if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGB)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGBNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
+	}
+	else
+	{
+		UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
+	}
+	return cloud;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);

corelib/src/util3d_surface.cpp

@@ -1196,6 +1196,51 @@ void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & image
 	}
 }
 
+std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
+{
+	std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p] = polygons[p].vertices;
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
+{
+	std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p] = tex_polygons[t][p].vertices;
+		}
+	}
+	return polygonsOut;
+}
+std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
+{
+	std::vector<pcl::Vertices> polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p].vertices = polygons[p];
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
+{
+	std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p].vertices = tex_polygons[t][p];
+		}
+	}
+	return polygonsOut;
+}
+
 pcl::TextureMesh::Ptr assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,

guilib/include/rtabmap/gui/CloudViewer.h

@@ -222,6 +222,19 @@ public:
 	void removeAllSpheres();
 	const std::set<std::string> & getAddedSpheres() const {return _spheres;}
 
+	void addOrUpdateCube(
+				const std::string & id,
+				const Transform & pose, // center of the cube
+				float width,  // e.g., along x axis
+				float height, // e.g., along y axis
+				float depth,  // e.g., along z axis
+				const QColor & color,
+				bool wireframe = false,
+				bool foreground = false);
+	void removeCube(const std::string & id);
+	void removeAllCubes();
+	const std::set<std::string> & getAddedCubes() const {return _cubes;}
+
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
@@ -314,7 +327,7 @@ public:
 	float getNormalsScale() const;
 	void setNormalsStep(int step);
 	void setNormalsScale(float scale);
-	void buildLocator(bool enable);
+	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
 
 public slots:
@@ -376,6 +389,7 @@ private:
     std::set<std::string> _texts;
     std::set<std::string> _lines;
     std::set<std::string> _spheres;
+    std::set<std::string> _cubes;
     QMap<std::string, Transform> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -95,6 +95,9 @@ private slots:
 	void generateGraph();
 	void exportSaved2DMap();
 	void import2DMap();
+	void viewOptimizedMesh();
+	void exportOptimizedMesh();
+	void updateOptimizedMesh();
 	void exportDatabase();
 	void extractImages();
 	void exportPosesRaw();

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -64,6 +64,10 @@ public:
 	void loadSettings(QSettings & settings, const QString & group = "");
 
 	void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
+	void forceAssembling(bool enabled);
+	void setProgressDialogToMax();
+	void setSaveButton();
+	void setOkButton();
 
 	void exportClouds(
 			const std::map<int, Transform> & poses,
@@ -85,6 +89,31 @@ public:
 			const QString & workingDirectory,
 			const ParametersMap & parameters);
 
+	bool getExportedClouds(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, int> & mapIds,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const QString & workingDirectory,
+			const ParametersMap & parameters,
+			std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+			std::map<int, pcl::PolygonMesh::Ptr> & meshes,
+			std::map<int, pcl::TextureMesh::Ptr> & textureMeshes,
+			std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
+
+	int getTextureSize() const;
+	int getMaxTextures() const;
+	bool isGainCompensation() const;
+	double getGainBeta() const;
+	bool isGainRGB() const;
+	bool isBlending() const;
+	int getBlendingDecimation() const;
+	int getTextureBrightnessConstrastRatioLow() const;
+	int getTextureBrightnessConstrastRatioHigh() const;
+	bool isExposeFusion() const;
+
 	static bool removeDirRecursively(const QString & dirName);
 
 signals:
@@ -109,26 +138,10 @@ private:
 			const std::map<int, LaserScan> & cachedScans,
 			const ParametersMap & parameters,
 			bool & has2dScans) const;
-	bool getExportedClouds(
-				const std::map<int, Transform> & poses,
-				const std::multimap<int, Link> & links,
-				const std::map<int, int> & mapIds,
-				const QMap<int, Signature> & cachedSignatures,
-				const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
-				const std::map<int, LaserScan> & cachedScans,
-				const QString & workingDirectory,
-				const ParametersMap & parameters,
-				std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
-				std::map<int, pcl::PolygonMesh::Ptr> & meshes,
-				std::map<int, pcl::TextureMesh::Ptr> & textureMeshes,
-				std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
 	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
 	void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
 	void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
 
-	void setSaveButton();
-	void setOkButton();
-
 private:
 	Ui_ExportCloudsDialog * _ui;
 	ProgressDialog * _progressDialog;

guilib/src/CloudViewer.cpp

@@ -82,6 +82,8 @@ class MyInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
 public:
 	MyInteractorStyle(CloudViewer * viewer) :
 		pcl::visualization::PCLVisualizerInteractorStyle(),
+		NumberOfClicks(0),
+		ResetPixelDistance(0),
 		pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
 		viewer_(viewer)
 	{
@@ -239,7 +241,7 @@ protected:
 				this->NumberOfClicks = 1;
 			}
 
-			if(this->NumberOfClicks == 2)
+			if(this->NumberOfClicks >= 2)
 			{
 				this->NumberOfClicks = 0;
 				this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
@@ -352,8 +354,6 @@ private:
 	unsigned int NumberOfClicks;
 	int PreviousPosition[2];
 	int ResetPixelDistance;
-	vtkSmartPointer<vtkTextActor> textActor_;
-	vtkSmartPointer<vtkLine> lineActor_;
 	float PreviousMeasure[3];
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
 	CloudViewer * viewer_;
@@ -1727,6 +1727,68 @@ void CloudViewer::removeAllSpheres()
 	UASSERT(_spheres.empty());
 }
 
+void CloudViewer::addOrUpdateCube(
+			const std::string & id,
+			const Transform & pose,
+			float width,
+			float height,
+			float depth,
+			const QColor & color,
+			bool wireframe,
+			bool foreground)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeCube(id);
+
+	if(!pose.isNull())
+	{
+		_cubes.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+		_visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?2:1);
+		if(wireframe)
+		{
+			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
+		}
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+	}
+}
+
+void CloudViewer::removeCube(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_cubes.find(id) != _cubes.end())
+	{
+		_visualizer->removeShape(id);
+		_cubes.erase(id);
+	}
+}
+
+void CloudViewer::removeAllCubes()
+{
+	std::set<std::string> cubes = _cubes;
+	for(std::set<std::string>::iterator iter = cubes.begin(); iter!=cubes.end(); ++iter)
+	{
+		this->removeCube(*iter);
+	}
+	UASSERT(_cubes.empty());
+}
+
 static const float frustum_vertices[] = {
     0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
@@ -2706,7 +2768,7 @@ void CloudViewer::setNormalsScale(float scale)
 	}
 }
 
-void CloudViewer::buildLocator(bool enable)
+void CloudViewer::buildPickingLocator(bool enable)
 {
 	_buildLocator = enable;
 }

guilib/src/DatabaseViewer.cpp

@@ -82,6 +82,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RecoveryState.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
@@ -248,6 +249,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
 	connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
 	connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
+	connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
+	connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
+	connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -276,6 +280,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->menuExport_GPS->setEnabled(false);
 	ui_->actionPoses_KML->setEnabled(false);
 	ui_->actionExport_saved_2D_map->setEnabled(false);
+	ui_->actionView_optimized_mesh->setEnabled(false);
+	ui_->actionExport_optimized_mesh->setEnabled(false);
+	ui_->actionUpdate_optimized_mesh->setEnabled(false);
 
 	ui_->horizontalSlider_A->setTracking(false);
 	ui_->horizontalSlider_B->setTracking(false);
@@ -938,6 +945,9 @@ bool DatabaseViewer::closeDatabase()
 		ui_->actionPoses_KML->setEnabled(false);
 		ui_->actionExport_saved_2D_map->setEnabled(false);
 		ui_->actionImport_2D_map->setEnabled(false);
+		ui_->actionView_optimized_mesh->setEnabled(false);
+		ui_->actionExport_optimized_mesh->setEnabled(false);
+		ui_->actionUpdate_optimized_mesh->setEnabled(false);
 		ui_->checkBox_showOptimized->setEnabled(false);
 		ui_->toolBox_statistics->clear();
 		databaseFileName_.clear();
@@ -1478,6 +1488,9 @@ void DatabaseViewer::updateIds()
 	ui_->actionPoses_KML->setEnabled(false);
 	ui_->actionExport_saved_2D_map->setEnabled(false);
 	ui_->actionImport_2D_map->setEnabled(false);
+	ui_->actionView_optimized_mesh->setEnabled(false);
+	ui_->actionExport_optimized_mesh->setEnabled(false);
+	ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
 	links_.clear();
 	linksAdded_.clear();
 	linksRefined_.clear();
@@ -1643,6 +1656,12 @@ void DatabaseViewer::updateIds()
 	ui_->actionExport_saved_2D_map->setEnabled(hasMap);
 	ui_->actionImport_2D_map->setEnabled(hasMap);
 
+	if(!dbDriver_->loadOptimizedMesh().empty())
+	{
+		ui_->actionView_optimized_mesh->setEnabled(true);
+		ui_->actionExport_optimized_mesh->setEnabled(true);
+	}
+
 	UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
 
 	if(ids_.size() && ui_->toolBox_statistics->isVisible())
@@ -2418,6 +2437,300 @@ void DatabaseViewer::import2DMap()
 	}
 }
 
+void DatabaseViewer::viewOptimizedMesh()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
+	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
+	cv::Mat textures;
+	cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
+	if(cloudMat.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
+	}
+	else
+	{
+		CloudViewer * viewer = new CloudViewer(this);
+		viewer->setWindowFlags(Qt::Window);
+		viewer->setAttribute(Qt::WA_DeleteOnClose);
+		viewer->buildPickingLocator(true);
+		if(!textures.empty())
+		{
+			pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+			util3d::fixTextureMeshForVisualization(*mesh);
+			viewer->setWindowTitle("Optimized Textured Mesh");
+			viewer->setPolygonPicking(true);
+			viewer->addCloudTextureMesh("mesh", mesh, textures);
+		}
+		else if(polygons.size() == 1)
+		{
+			pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
+			viewer->setWindowTitle("Optimized Mesh");
+			viewer->setPolygonPicking(true);
+			viewer->addCloudMesh("mesh", mesh);
+		}
+		else
+		{
+			LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
+			pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
+			viewer->setWindowTitle("Optimized Point Cloud");
+			viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
+		}
+		viewer->show();
+	}
+}
+
+void DatabaseViewer::exportOptimizedMesh()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
+	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
+	cv::Mat textures;
+	cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
+	if(cloudMat.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
+	}
+	else
+	{
+		QString name = QFileInfo(databaseFileName_.c_str()).baseName();
+
+		if(!textures.empty())
+		{
+			pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".obj",
+					tr("Mesh (*.obj)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".obj";
+				}
+				QString baseName = QFileInfo(path).baseName();
+				if(mesh->tex_materials.size() == 1)
+				{
+					mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
+					cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
+				}
+				else
+				{
+					for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
+					{
+						mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
+						UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
+						cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
+					}
+				}
+				pcl::io::saveOBJFile(path.toStdString(), *mesh);
+
+				QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
+			}
+		}
+		else if(polygons.size() == 1)
+		{
+			pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".ply",
+					tr("Mesh (*.ply)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".ply";
+				}
+				pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
+				QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
+			}
+		}
+		else
+		{
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".ply",
+					tr("Point cloud data (*.ply *.pcd)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".ply";
+				}
+				bool success = false;
+				pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
+				if(QFileInfo(path).suffix() == "pcd")
+				{
+					success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
+				}
+				else
+				{
+					success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
+				}
+				if(success)
+				{
+					QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
+				}
+				else
+				{
+					QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
+				}
+			}
+		}
+	}
+}
+
+void DatabaseViewer::updateOptimizedMesh()
+{
+	if(!ids_.size() || !dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+
+	if(graphes_.empty())
+	{
+		this->updateGraphView();
+		if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
+		{
+			QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
+			return;
+		}
+	}
+
+	std::map<int, Transform> optimizedPoses;
+	if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
+	{
+		optimizedPoses = groundTruthPoses_;
+	}
+	else
+	{
+		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+	}
+	if(ui_->groupBox_posefiltering->isChecked())
+	{
+		optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+				ui_->doubleSpinBox_posefilteringRadius->value(),
+				ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+	}
+	if(optimizedPoses.size() > 0)
+	{
+		exportDialog_->setDBDriver(dbDriver_);
+		exportDialog_->forceAssembling(true);
+		exportDialog_->setOkButton();
+
+		std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
+		std::map<int, pcl::PolygonMesh::Ptr> meshes;
+		std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
+		std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
+
+		if(exportDialog_->getExportedClouds(
+				optimizedPoses,
+				updateLinksWithModifications(links_),
+				mapIds_,
+				QMap<int, Signature>(),
+				std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
+				std::map<int, LaserScan>(),
+				pathDatabase_,
+				ui_->parameters_toolbox->getParameters(),
+				clouds,
+				meshes,
+				textureMeshes,
+				textureVertexToPixels))
+		{
+			if(textureMeshes.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+
+				cv::Mat globalTextures;
+				pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
+				if(textureMesh->tex_materials.size()>1)
+				{
+					globalTextures = util3d::mergeTextures(
+							*textureMesh,
+							std::map<int, cv::Mat>(),
+							std::map<int, std::vector<CameraModel> >(),
+							0,
+							dbDriver_,
+							exportDialog_->getTextureSize(),
+							exportDialog_->getMaxTextures(),
+							textureVertexToPixels,
+							exportDialog_->isGainCompensation(),
+							exportDialog_->getGainBeta(),
+							exportDialog_->isGainRGB(),
+							exportDialog_->isBlending(),
+							exportDialog_->getBlendingDecimation(),
+							exportDialog_->getTextureBrightnessConstrastRatioLow(),
+							exportDialog_->getTextureBrightnessConstrastRatioHigh(),
+							exportDialog_->isExposeFusion());
+				}
+				dbDriver_->saveOptimizedMesh(
+						util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
+						util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
+						textureMesh->tex_coordinates,
+						globalTextures);
+				QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else if(meshes.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+				polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
+				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
+				QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else if(clouds.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
+				QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else
+			{
+				QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
+			}
+		}
+		exportDialog_->setProgressDialogToMax();
+	}
+	else
+	{
+		QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
+	}
+}
+
 void DatabaseViewer::generateGraph()
 {
 	if(!dbDriver_)

guilib/src/ExportCloudsDialog.cpp

@@ -273,6 +273,24 @@ void ExportCloudsDialog::cancel()
 	_progressDialog->appendText(tr("Canceled!"));
 }
 
+void ExportCloudsDialog::forceAssembling(bool enabled)
+{
+	if(enabled)
+	{
+		_ui->checkBox_assemble->setChecked(true);
+		_ui->checkBox_assemble->setEnabled(false);
+	}
+	else
+	{
+		_ui->checkBox_assemble->setEnabled(true);
+	}
+}
+
+void ExportCloudsDialog::setProgressDialogToMax()
+{
+	_progressDialog->setValue(_progressDialog->maximumSteps());
+}
+
 void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & group) const
 {
 	if(!group.isEmpty())
@@ -433,7 +451,10 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble());
 	_ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value()).toInt());
 
-	_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
+	if(_ui->checkBox_assemble->isEnabled())
+	{
+		_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
+	}
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble());
 	_ui->comboBox_frame->setCurrentIndex(settings.value("frame", _ui->comboBox_frame->currentIndex()).toInt());
 
@@ -956,7 +977,7 @@ void ExportCloudsDialog::viewClouds(
 		}
 		viewer->setLighting(true);
 		viewer->setDefaultBackgroundColor(QColor(40, 40, 40, 255));
-		viewer->buildLocator(true);
+		viewer->buildPickingLocator(true);
 
 		QVBoxLayout *layout = new QVBoxLayout();
 		layout->addWidget(viewer);
@@ -1187,6 +1208,60 @@ void ExportCloudsDialog::viewClouds(
 	_progressDialog->setValue(_progressDialog->maximumSteps());
 }
 
+int ExportCloudsDialog::getTextureSize() const
+{
+	int textureSize = 1024;
+	if(_ui->comboBox_meshingTextureSize->currentIndex() > 0)
+	{
+		textureSize = 128 << _ui->comboBox_meshingTextureSize->currentIndex(); // start at 256
+	}
+	return textureSize;
+}
+int ExportCloudsDialog::getMaxTextures() const
+{
+	return _ui->spinBox_mesh_maxTextures->value();
+}
+bool ExportCloudsDialog::isGainCompensation() const
+{
+	return _ui->checkBox_gainCompensation->isChecked();
+}
+double ExportCloudsDialog::getGainBeta() const
+{
+	return _ui->doubleSpinBox_gainBeta->value();
+}
+bool ExportCloudsDialog::isGainRGB() const
+{
+	return _ui->checkBox_gainRGB->isChecked();
+}
+bool ExportCloudsDialog::isBlending() const
+{
+	return _ui->checkBox_blending->isChecked();
+}
+int ExportCloudsDialog::getBlendingDecimation() const
+{
+	int blendingDecimation = 0;
+	if(_ui->checkBox_blending->isChecked())
+	{
+		if(_ui->comboBox_blendingDecimation->currentIndex() > 0)
+		{
+			blendingDecimation = 1 << (_ui->comboBox_blendingDecimation->currentIndex()-1);
+		}
+	}
+	return blendingDecimation;
+}
+int ExportCloudsDialog::getTextureBrightnessConstrastRatioLow() const
+{
+	return _ui->spinBox_textureBrightnessContrastRatioLow->value();
+}
+int ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh() const
+{
+	return _ui->spinBox_textureBrightnessContrastRatioHigh->value();
+}
+bool ExportCloudsDialog::isExposeFusion() const
+{
+	return _ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked();
+}
+
 bool ExportCloudsDialog::removeDirRecursively(const QString & dirName)
 {
     bool result = true;
@@ -2196,7 +2271,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						poisson.reconstruct(*mesh);
 					}
 
-					_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(cloudsAdded).arg(clouds.size()));
+					_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 					QApplication::processEvents();
 
 					if(mesh->polygons.size()>0)

guilib/src/ui/DatabaseViewer.ui

@@ -621,6 +621,10 @@
     <addaction name="actionExport_saved_2D_map"/>
     <addaction name="actionImport_2D_map"/>
     <addaction name="separator"/>
+    <addaction name="actionView_optimized_mesh"/>
+    <addaction name="actionUpdate_optimized_mesh"/>
+    <addaction name="actionExport_optimized_mesh"/>
+    <addaction name="separator"/>
     <addaction name="actionQuit"/>
    </widget>
    <widget class="QMenu" name="menuEdit">
@@ -2640,6 +2644,21 @@
     <string>Import 2D map...</string>
    </property>
   </action>
+  <action name="actionView_optimized_mesh">
+   <property name="text">
+    <string>View optimized mesh</string>
+   </property>
+  </action>
+  <action name="actionExport_optimized_mesh">
+   <property name="text">
+    <string>Export optimized mesh...</string>
+   </property>
+  </action>
+  <action name="actionUpdate_optimized_mesh">
+   <property name="text">
+    <string>Update optimized mesh...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>