-Added the grid map in the 3D map view (pcl::TextureMesh, available only when pcl>=1.7.2). Refactored how 2d maps are created.

-Fixed export grid map on Mac OS X.


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

corelib/include/rtabmap/core/util3d.h

@@ -400,6 +400,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		cv::Mat & grid,
 		bool stopOnObstacle);
 
+cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/util3d.cpp

@@ -2056,6 +2056,35 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 	}
 }
 
+//convert to gray scaled map
+cv::Mat convertMap2Image8U(const cv::Mat & map8S)
+{
+	UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
+	cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
+	for (int i = 0; i < map8S.rows; ++i)
+	{
+		for (int j = 0; j < map8S.cols; ++j)
+		{
+			char v = map8S.at<char>(i, j);
+			unsigned char gray;
+			if(v == 0)
+			{
+				gray = 178;
+			}
+			else if(v == 100)
+			{
+				gray = 0;
+			}
+			else // -1
+			{
+				gray = 89;
+			}
+			map8U.at<unsigned char>(i, j) = gray;
+		}
+	}
+	return map8U;
+}
+
 }
 
 }

guilib/include/rtabmap/gui/CloudViewer.h

@@ -20,6 +20,8 @@
 #include <QtCore/QSet>
 #include <QtCore/qnamespace.h>
 
+#include <opencv2/opencv.hpp>
+
 #include <pcl/visualization/mouse_event.h>
 #include <pcl/PCLPointCloud2.h>
 
@@ -92,6 +94,14 @@ public:
 		const pcl::PolygonMesh::Ptr & mesh,
 		const Transform & pose = Transform::getIdentity());
 
+	bool addOccupancyGridMap(
+			const cv::Mat & map8U,
+			float resolution, // cell size
+			float xMin,
+			float yMin,
+			float opacity);
+	void removeOccupancyGridMap();
+
 	void updateCameraPosition(
 		const Transform & pose);
 
@@ -111,6 +121,7 @@ public:
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
 	void setGridShown(bool shown);
+	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
 
 public slots:
 	void render();
@@ -150,6 +161,7 @@ private:
     Transform _lastPose;
     std::list<std::string> _gridLines;
     QSet<Qt::Key> _keysPressed;
+    QString _workingDirectory;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -30,6 +30,8 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <set>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 #include <rtabmap/core/Link.h>
 
@@ -92,7 +94,7 @@ private:
 	std::list<std::map<int, rtabmap::Transform> > graphes_;
 	std::map<int, rtabmap::Transform> poses_;
 	std::multimap<int, rtabmap::Link> links_;
-	QMap<int, std::vector<unsigned char> > scans_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
 };
 
 #endif /* DATABASEVIEWER_H_ */

guilib/include/rtabmap/gui/MainWindow.h

@@ -176,7 +176,7 @@ signals:
 
 private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
-	void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
+	void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints);
 	void createAndAddCloudToMap(int nodeId, const Transform & pose);
 	void createAndAddScanToMap(int nodeId, const Transform & pose);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
@@ -248,6 +248,7 @@ private:
 	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
 	QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _lastOdometryProcessed;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -134,6 +134,11 @@ public:
 	double getCloudFilteringRadius() const;
 	double getCloudFilteringAngle() const;
 
+	bool getGridMapShown() const;
+	double getGridMapResolution() const;
+	bool getGridMapFillEmptySpace() const;
+	double getGridMapOpacity() const;
+
 	QString getWorkingDirectory() const;
 	QString getDatabasePath() const;
 

guilib/include/rtabmap/gui/UCv2Qt.h

@@ -65,6 +65,9 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
 		{
 			// mono grayscale
 			qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
+			QVector<QRgb> my_table;
+			for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+			qtemp.setColorTable(my_table);
 		}
 		else
 		{

guilib/src/CloudViewer.cpp

@@ -54,7 +54,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
-		_lastPose(Transform::getIdentity())
+		_lastPose(Transform::getIdentity()),
+		_workingDirectory(".")
 {
 	this->setMinimumSize(200, 200);
 
@@ -322,6 +323,80 @@ bool CloudViewer::addCloudMesh(
 	return false;
 }
 
+bool CloudViewer::addOccupancyGridMap(
+		const cv::Mat & map8U,
+		float resolution, // cell size
+		float xMin,
+		float yMin,
+		float opacity)
+{
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+	UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
+
+	float xSize = float(map8U.cols) * resolution;
+	float ySize = float(map8U.rows) * resolution;
+
+	UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
+	if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+
+	if(xSize > 0.0f && ySize > 0.0f)
+	{
+		pcl::TextureMeshPtr mesh(new pcl::TextureMesh());
+		pcl::PointCloud<pcl::PointXYZ> cloud;
+		cloud.push_back(pcl::PointXYZ(xMin,       yMin,       0));
+		cloud.push_back(pcl::PointXYZ(xSize+xMin, yMin,       0));
+		cloud.push_back(pcl::PointXYZ(xSize+xMin, ySize+yMin, 0));
+		cloud.push_back(pcl::PointXYZ(xMin,       ySize+yMin, 0));
+		pcl::toPCLPointCloud2(cloud, mesh->cloud);
+
+		std::vector<pcl::Vertices> polygons(1);
+		polygons[0].vertices.push_back(0);
+		polygons[0].vertices.push_back(1);
+		polygons[0].vertices.push_back(2);
+		polygons[0].vertices.push_back(3);
+		polygons[0].vertices.push_back(0);
+		mesh->tex_polygons.push_back(polygons);
+
+		// default texture materials parameters
+		pcl::TexMaterial material;
+		// hack, can we read from memory?
+		std::string tmpPath = (_workingDirectory+"/.tmp_map.png").toStdString();
+		cv::imwrite(tmpPath, map8U);
+		material.tex_file = tmpPath;
+		mesh->tex_materials.push_back(material);
+
+		std::vector<Eigen::Vector2f> coordinates;
+		coordinates.push_back(Eigen::Vector2f(0,1));
+		coordinates.push_back(Eigen::Vector2f(1,1));
+		coordinates.push_back(Eigen::Vector2f(1,0));
+		coordinates.push_back(Eigen::Vector2f(0,0));
+		mesh->tex_coordinates.push_back(coordinates);
+
+		_visualizer->addTextureMesh(*mesh, "map");
+		_visualizer->getCloudActorMap()->find("map")->second.actor->GetProperty()->LightingOff();
+		setCloudOpacity("map", 0.7);
+
+		//removed tmp texture file
+		QFile::remove(tmpPath.c_str());
+	}
+	return true;
+#else
+	// not implemented on lower version of PCL
+	return false;
+#endif
+}
+
+void CloudViewer::removeOccupancyGridMap()
+{
+	if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+}
+
 void CloudViewer::setTrajectoryShown(bool shown)
 {
 	_aShowTrajectory->setChecked(shown);
@@ -448,8 +523,13 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 				cameras.front().view[2] = Fp[10];
 			}
 
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			_visualizer->removeCoordinateSystem("reference", 0);
+			_visualizer->addCoordinateSystem(0.2, m, "reference", 0);
+#else
 			_visualizer->removeCoordinateSystem(0);
 			_visualizer->addCoordinateSystem(0.2, m, 0);
+#endif
 			_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],

guilib/src/DatabaseViewer.cpp

@@ -914,13 +914,23 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				memory_->getImageDepth(ids_.at(i), imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
 				if(depth2dBytes.size())
 				{
-					scans_.insert(ids_.at(i), depth2dBytes);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+					cv::Mat depth2d = rtabmap::util3d::uncompressData(depth2dBytes);
+					cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
+					scans_.insert(std::make_pair(ids_.at(i), cloud));
 				}
 			}
 			UINFO("Update scans list... done");
 		}
 		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
-		ui_->graphViewer->updateGraph(graph, links_, scans_);
+		ui_->graphViewer->updateGraph(graph, links_);
+		if(graph.size() && scans_.size())
+		{
+			float xMin, yMin;
+			float cell = 0.05;
+			cv::Mat map = rtabmap::util3d::convertMap2Image8U(rtabmap::util3d::create2DMap(graph, scans_, cell, true, xMin, yMin));
+			ui_->graphViewer->updateMap(map, cell, xMin, yMin);
+		}
 		ui_->label_iterations->setNum(value);
 
 		//compute total length (neighbor links)

guilib/src/GraphViewer.cpp

@@ -27,6 +27,7 @@
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/gui/UCv2Qt.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap {
 
@@ -34,11 +35,10 @@ class NodeItem: public QGraphicsEllipseItem
 {
 public:
 	// in meter
-	NodeItem(int id, const Transform & pose, float radius, const cv::Mat & scan) :
+	NodeItem(int id, const Transform & pose, float radius) :
 		QGraphicsEllipseItem(QRectF(-radius,-radius,radius*2.0f,radius*2.0f)),
 		_id(id),
-		_pose(pose),
+		_pose(pose)
-		_scan(scan)
 	{
 		this->setPos(-pose.y(),-pose.x());
 		this->setBrush(pen().color());
@@ -56,8 +56,6 @@ public:
 	}
 
 	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 )
@@ -77,7 +75,6 @@ protected:
 private:
 	int _id;
 	Transform _pose;
-	cv::Mat _scan;
 };
 
 class LinkItem: public QGraphicsLineItem
@@ -136,11 +133,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_linkWidth(0),
 		_gridMap(0),
 		_lastReferential(0),
-		_gridCellSize(0.05f),
+		_gridCellSize(0.0f)
-		_gridUnknownSpaceFilled(true)
 {
-	Q_ASSERT(_gridCellSize > 0);
-
 	this->setScene(new QGraphicsScene(this));
 	this->setDragMode(QGraphicsView::ScrollHandDrag);
 	_workingDirectory = QDir::homePath();
@@ -170,8 +164,6 @@ GraphViewer::GraphViewer(QWidget * parent) :
 
 
 	_gridMap = this->scene()->addPixmap(QPixmap());
-	_gridMap->scale(_gridCellSize, -_gridCellSize);
-	_gridMap->setRotation(90);
 	_gridMap->setZValue(0);
 	_gridMap->setParentItem(_root);
 }
@@ -181,8 +173,7 @@ GraphViewer::~GraphViewer()
 }
 
 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
-				 const std::multimap<int, Link> & constraints,
+				 const std::multimap<int, Link> & constraints)
-				 const QMap<int, std::vector<unsigned char> > & scans)
 {
 	//Hide nodes and links
 	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -199,40 +190,19 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		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() && scans.contains(iter->first))
-			{
-				cv::Mat depth2d = util3d::uncompressData(scans.value(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
-			QMap<int, std::vector<unsigned char> >::const_iterator jter = scans.find(iter->first);
-			cv::Mat depth2d;
-			if(jter != scans.end())
-			{
-				depth2d = util3d::uncompressData(jter.value());
-				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);
+			NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius);
 			this->scene()->addItem(item);
 			item->setZValue(2);
 			item->setColor(_nodeColor);
@@ -324,41 +294,6 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		}
 	}
 
-	if(scanClouds.size())
-	{
-		float xMin=0.0f, yMin=0.0f;
-		cv::Mat map8S = util3d::create2DMap(poses, scanClouds, _gridCellSize, _gridUnknownSpaceFilled, 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 v = map8S.at<char>(i, j);
-				unsigned char gray;
-				if(v == 0)
-				{
-					gray = 178;
-				}
-				else if(v == 100)
-				{
-					gray = 0;
-				}
-				else // -1
-				{
-					gray = 89;
-				}
-				map8U.at<unsigned char>(i, j) = gray;
-			}
-		}
-		QImage image = uCvMat2QImage(map8U, false);
-		_gridMap->resetTransform();
-		_gridMap->scale(_gridCellSize, -_gridCellSize);
-		_gridMap->setRotation(90);
-		_gridMap->setPixmap(QPixmap::fromImage(image));
-		_gridMap->setPos(-yMin, -xMin);
-	}
-
 	if(_nodeItems.size())
 	{
 		(--_nodeItems.end()).value()->setColor(Qt::green);
@@ -374,6 +309,25 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
 }
 
+void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
+{
+	UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f));
+	if(!map8U.empty())
+	{
+		_gridCellSize = resolution;
+		QImage image = uCvMat2QImage(map8U, false);
+		_gridMap->resetTransform();
+		_gridMap->scale(resolution, -resolution);
+		_gridMap->setRotation(90);
+		_gridMap->setPixmap(QPixmap::fromImage(image));
+		_gridMap->setPos(-yMin, -xMin);
+	}
+	else
+	{
+		this->clearMap();
+	}
+}
+
 void GraphViewer::clearGraph()
 {
 	qDeleteAll(_nodeItems);
@@ -382,11 +336,21 @@ void GraphViewer::clearGraph()
 	_neighborLinkItems.clear();
 	qDeleteAll(_loopLinkItems);
 	_loopLinkItems.clear();
-	_gridMap->setPixmap(QPixmap());
 	_lastReferential->resetTransform();
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
 }
 
+void GraphViewer::clearMap()
+{
+	_gridMap->setPixmap(QPixmap());
+}
+
+void GraphViewer::clearAll()
+{
+	clearMap();
+	clearGraph();
+}
+
 void GraphViewer::wheelEvent ( QWheelEvent * event )
 {
 	if(event->delta() > 0)
@@ -412,10 +376,6 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
 	QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
 	menu.addSeparator();
-	QAction * aSetGridCellSize = menu.addAction(tr("Set grid cell size..."));
-	QAction * aSetGridUnknownSpaceFilled = menu.addAction(tr("Unknown grid space filled"));
-	aSetGridUnknownSpaceFilled->setCheckable(true);
-	aSetGridUnknownSpaceFilled->setChecked(_gridUnknownSpaceFilled);
 	QAction * aShowHideGridMap;
 	if(_gridMap->isVisible())
 	{
@@ -448,7 +408,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (isPNG?".png":".svg"));
 
 			//_root->setScale(this->transform().m11()); // current view
+			if(_gridCellSize)
+			{
 				_root->setScale(1.0f/_gridCellSize); // grid map precision (for 5cm grid cell, x20 to have 1pix/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
@@ -575,19 +538,6 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			}
 		}
 	}
-	else if(r == aSetGridCellSize)
-	{
-		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Width (m)"), _gridCellSize, 0.01, 10, 2, &ok);
-		if(ok)
-		{
-			_gridCellSize = value;
-		}
-	}
-	else if(r == aSetGridUnknownSpaceFilled)
-	{
-		_gridUnknownSpaceFilled = aSetGridUnknownSpaceFilled->isChecked();
-	}
 	else if(r == aShowHideGridMap)
 	{
 		_gridMap->setVisible(!_gridMap->isVisible());

guilib/src/GraphViewer.h

@@ -11,6 +11,7 @@
 #include <QtGui/QGraphicsView>
 #include <QtCore/QMap>
 #include <rtabmap/core/Link.h>
+#include <opencv2/opencv.hpp>
 #include <map>
 
 class QGraphicsItem;
@@ -28,12 +29,13 @@ public:
 	virtual ~GraphViewer();
 
 	void updateGraph(const std::map<int, Transform> & poses,
-					 const std::multimap<int, Link> & constraints,
+					 const std::multimap<int, Link> & constraints);
-					 const QMap<int, std::vector<unsigned char> > & scans);
+	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void clearGraph();
+	void clearMap();
+	void clearAll();
 
 	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
-	void setGridCellSize(float size) {Q_ASSERT(_gridCellSize>0.0f); _gridCellSize= size;}
 
 protected:
 	virtual void wheelEvent ( QWheelEvent * event );
@@ -53,7 +55,6 @@ private:
 	QGraphicsPixmapItem * _gridMap;
 	QGraphicsItemGroup * _lastReferential;
 	float _gridCellSize;
-	bool _gridUnknownSpaceFilled;
 };
 
 } /* namespace rtabmap */

guilib/src/MainWindow.cpp

@@ -342,6 +342,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 	_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
+	_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 
 	splash.close();
 
@@ -953,27 +954,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.poses().size())
 		{
 			// update pose only if a odometry is not received
-			updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose());
+			updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose(), stat.constraints());
-
-			// update some widgets
-			if(_ui->graphicsView_graphView->isVisible())
-			{
-				std::map<int, Transform> poses;
-				if(_preferencesDialog->isCloudFiltering() && stat.poses().size())
-				{
-					float radius = _preferencesDialog->getCloudFilteringRadius();
-					float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
-					poses = util3d::radiusPosesFiltering(stat.poses(), radius, angle);
-					// make sure the last is here
-					poses.insert(*stat.poses().rbegin());
-				}
-				else
-				{
-					poses = stat.poses();
-				}
-
-				_ui->graphicsView_graphView->updateGraph(poses, stat.constraints(), _depths2DMap);
-			}
 
 			_odometryReceived = false;
 
@@ -1055,7 +1036,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	_processingStatistics = false;
 }
 
-void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const Transform & currentPose)
+void MainWindow::updateMapCloud(
+		const std::map<int, Transform> & posesIn,
+		const Transform & currentPose,
+		const std::multimap<int, Link> & constraints)
 {
 	if(posesIn.size())
 	{
@@ -1140,7 +1124,7 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
 
 			// 2d point cloud
 			std::string scanName = uFormat("scan%d", iter->first);
-			if(_preferencesDialog->isScansShown(0))
+			if(_preferencesDialog->isScansShown(0) || _ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())
 			{
 				if(viewerClouds.contains(scanName))
 				{
@@ -1162,6 +1146,11 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
 				{
 					this->createAndAddScanToMap(iter->first, iter->second);
 				}
+				if(!_preferencesDialog->isScansShown(0))
+				{
+					UDEBUG("Hide scan %s", scanName.c_str());
+					_ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
+				}
 			}
 			else if(viewerClouds.contains(scanName))
 			{
@@ -1192,6 +1181,39 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
 		}
 	}
 
+	// Update occupancy grid map in 3D map view and graph view
+	if(_ui->graphicsView_graphView->isVisible() && constraints.size())
+	{
+		_ui->graphicsView_graphView->updateGraph(poses, constraints);
+	}
+	cv::Mat map8U;
+	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && _depths2DMap.size())
+	{
+		float xMin, yMin;
+		float resolution = _preferencesDialog->getGridMapResolution();
+		bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
+		cv::Mat map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
+		if(!map8S.empty())
+		{
+			//convert to gray scaled map
+			map8U = util3d::convertMap2Image8U(map8S);
+
+			if(_preferencesDialog->getGridMapShown())
+			{
+				float opacity = _preferencesDialog->getGridMapOpacity();
+				_ui->widget_cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
+			}
+			if(_ui->graphicsView_graphView->isVisible())
+			{
+				_ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
+			}
+		}
+	}
+	if(!_preferencesDialog->getGridMapShown())
+	{
+		_ui->widget_cloudViewer->removeOccupancyGridMap();
+	}
+
 	if(viewerClouds.contains("cloudOdom"))
 	{
 		if(!_preferencesDialog->isCloudsShown(1))
@@ -1318,6 +1340,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
 	{
 		UERROR("Adding cloud %d to viewer failed!", nodeId);
 	}
+	else
+	{
+		_createdScans.insert(std::make_pair(nodeId, cloud));
+	}
 	_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 	_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 }
@@ -1502,19 +1528,8 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_initProgressDialog->appendText("Updating the 3D map cloud...");
 			_initProgressDialog->incrementStep();
 			QApplication::processEvents();
-			this->updateMapCloud(event.getPoses(), Transform());
+			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints());
 			_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(),
-						_depths2DMap);
-				_initProgressDialog->appendText("Updating the graph view... done.");
-			}
 		}
 		else
 		{
@@ -1590,7 +1605,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		UDEBUG("Cloud rendering settings changed...");
 		if(_currentPosesMap.size())
 		{
-			this->updateMapCloud(_currentPosesMap, Transform());
+			this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>());
 		}
 	}
 
@@ -2682,6 +2697,7 @@ void MainWindow::clearTheCache()
 	_depthCysMap.clear();
 	_localTransformsMap.clear();
 	_createdClouds.clear();
+	_createdScans.clear();
 	_ui->widget_cloudViewer->removeAllClouds();
 	_ui->widget_cloudViewer->render();
 	_currentPosesMap.clear();
@@ -2703,7 +2719,7 @@ void MainWindow::clearTheCache()
 	_ui->label_stats_loopClosuresRejected->setText("0");
 	_refIds.clear();
 	_loopClosureIds.clear();
-	_ui->graphicsView_graphView->clearGraph();
+	_ui->graphicsView_graphView->clearAll();
 }
 
 void MainWindow::updateElapsedTime()
@@ -2942,12 +2958,22 @@ void MainWindow::exportGridMap()
 				map8U.at<unsigned char>(i, j) = gray;
 			}
 		}
+
 		QImage image = uCvMat2QImage(map8U, false);
 
-		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.bmp *.png)"));
+		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
 		if(!path.isEmpty())
 		{
-			QPixmap::fromImage(image.mirrored(false, true).transformed(QTransform().rotate(-90))).save(path);
+			if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
+			{
+				//use png by default
+				path += ".png";
+			}
+
+			QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
+			QPixmap::fromImage(img).save(path);
+
+			QDesktopServices::openUrl(QUrl::fromLocalFile(path));
 		}
 	}
 }
@@ -3032,17 +3058,16 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
 	_initProgressDialog->setAutoClose(true, 1);
 	_initProgressDialog->resetProgress();
 	_initProgressDialog->show();
-	_initProgressDialog->setMaximumSteps(int(poses.size())*assemble?1:2+1);
+	_initProgressDialog->setMaximumSteps(int(poses.size())*(assemble?1:2)+1);
 
 	int count = 1;
 	int i = 0;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		bool inserted = false;
-		if(_depths2DMap.contains(iter->first))
+		if(_createdScans.find(iter->first) != _createdScans.end())
 		{
-			cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(iter->first));
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scan = _createdScans.at(iter->first);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::depth2DToPointCloud(depth2d);
 			if(scan->size())
 			{
 				if(assemble)
@@ -3316,7 +3341,7 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
 {
 	if(clouds.size() == 1)
 	{
-		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "cloud.ply", tr("Point cloud data (*.ply *.pcd)"));
+		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/cloud.ply", tr("Point cloud data (*.ply *.pcd)"));
 		if(!path.isEmpty())
 		{
 			if(clouds.begin()->second->size())
@@ -3424,7 +3449,7 @@ void MainWindow::saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes)
 {
 	if(meshes.size() == 1)
 	{
-		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "mesh.ply", tr("Mesh (*.ply)"));
+		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/mesh.ply", tr("Mesh (*.ply)"));
 		if(!path.isEmpty())
 		{
 			if(meshes.begin()->second->polygons.size())
@@ -3523,7 +3548,7 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
 {
 	if(scans.size() == 1)
 	{
-		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "scan.ply", tr("Point cloud data (*.ply *.pcd)"));
+		QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/scan.ply", tr("Point cloud data (*.ply *.pcd)"));
 		if(!path.isEmpty())
 		{
 			if(scans.begin()->second->size())

guilib/src/PreferencesDialog.cpp

@@ -96,6 +96,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->odom_bin_nn->removeItem(4);
 	}
 
+#if PCL_VERSION_COMPARE(<, 1, 7, 2)
+	_ui->checkBox_map_shown->setChecked(false);
+	_ui->checkBox_map_shown->setEnabled(false);
+	_ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)");
+#endif
+
 	_ui->predictionPlot->showLegend(false);
 
 	QButtonGroup * buttonGroup = new QButtonGroup(this);
@@ -180,6 +186,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_cloudFilterRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_cloudFilterAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
+	connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_map_fillEmptySpace, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+
 	//Logging panel
 	connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
 	connect(_ui->comboBox_loggerEventLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -742,6 +753,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->groupBox_poseFiltering->setChecked(false);
 		_ui->doubleSpinBox_cloudFilterRadius->setValue(0.1);
 		_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
+
+		_ui->checkBox_map_shown->setChecked(false);
+		_ui->doubleSpinBox_map_resolution->setValue(0.05);
+		_ui->checkBox_map_fillEmptySpace->setChecked(true);
+		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
 	{
@@ -972,6 +988,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_cloudFilterRadius->setValue(settings.value("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value()).toDouble());
 	_ui->doubleSpinBox_cloudFilterAngle->setValue(settings.value("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value()).toDouble());
 
+	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
+	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
+	_ui->checkBox_map_fillEmptySpace->setChecked(settings.value("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked()).toBool());
+	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
+
 	settings.endGroup(); // General
 
 	settings.endGroup(); // rtabmap
@@ -1178,6 +1199,10 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 	settings.setValue("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value());
 	settings.setValue("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value());
 
+	settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
+	settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
+	settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
+	settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
 	settings.endGroup(); // General
 
 	settings.endGroup(); // rtabmap
@@ -2467,6 +2492,23 @@ double PreferencesDialog::getCloudFilteringAngle() const
 {
 	return _ui->doubleSpinBox_cloudFilterAngle->value();
 }
+bool PreferencesDialog::getGridMapShown() const
+{
+	return _ui->checkBox_map_shown->isChecked();
+}
+double PreferencesDialog::getGridMapResolution() const
+{
+	return _ui->doubleSpinBox_map_resolution->value();
+}
+bool PreferencesDialog::getGridMapFillEmptySpace() const
+{
+	return _ui->checkBox_map_fillEmptySpace->isChecked();
+}
+double PreferencesDialog::getGridMapOpacity() const
+{
+	return _ui->doubleSpinBox_map_opacity->value();
+}
+
 
 // Source
 double PreferencesDialog::getGeneralInputRate() const

guilib/src/ui/exportCloudsDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>678</width>
-    <height>550</height>
+    <height>579</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -22,9 +22,7 @@
      <property name="checkable">
       <bool>true</bool>
      </property>
-     <layout class="QVBoxLayout" name="verticalLayout_3">
+     <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
-      <item>
-       <layout class="QFormLayout" name="formLayout_2">
       <item row="0" column="0">
        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
         <property name="suffix">
@@ -107,8 +105,6 @@
        </widget>
       </item>
      </layout>
-      </item>
-     </layout>
     </widget>
    </item>
    <item>
@@ -119,7 +115,7 @@
      <property name="checkable">
       <bool>true</bool>
      </property>
-     <layout class="QFormLayout" name="formLayout_3">
+     <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
       <item row="0" column="0">
        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
         <property name="suffix">
@@ -163,7 +159,7 @@
      <property name="checked">
       <bool>false</bool>
      </property>
-     <layout class="QVBoxLayout" name="verticalLayout_4">
+     <layout class="QVBoxLayout" name="verticalLayout">
       <item>
        <widget class="QLabel" name="label">
         <property name="text">
@@ -175,7 +171,7 @@
        </widget>
       </item>
       <item>
-       <layout class="QFormLayout" name="formLayout_4">
+       <layout class="QGridLayout" name="gridLayout_3" columnstretch="0,1">
         <item row="0" column="0">
          <widget class="QDoubleSpinBox" name="doubleSpinBox_mlsRadius">
           <property name="suffix">
@@ -222,10 +218,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
      <property name="checked">
       <bool>false</bool>
      </property>
-     <layout class="QFormLayout" name="formLayout">
+     <layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
-      <property name="fieldGrowthPolicy">
-       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-      </property>
       <item row="0" column="0">
        <widget class="QSpinBox" name="spinBox_normalKSearch">
         <property name="minimum">

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-185</y>
+            <y>0</y>
-            <width>737</width>
+            <width>732</width>
-            <height>893</height>
+            <height>1198</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -361,7 +361,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                         </widget>
                        </item>
                        <item>
-                        <layout class="QFormLayout" name="formLayout_22">
+                        <layout class="QGridLayout" name="gridLayout_17" rowstretch="0,0" columnstretch="0,1">
                          <item row="0" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
                            <property name="suffix">
@@ -382,13 +382,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_48">
-                           <property name="text">
-                            <string>Angle</string>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="1" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
                            <property name="suffix">
@@ -405,6 +398,122 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_48">
+                           <property name="text">
+                            <string>Angle</string>
+                           </property>
+                          </widget>
+                         </item>
+                        </layout>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_gridMap">
+                      <property name="title">
+                       <string>Occupancy grid map</string>
+                      </property>
+                      <property name="checkable">
+                       <bool>false</bool>
+                      </property>
+                      <property name="checked">
+                       <bool>false</bool>
+                      </property>
+                      <layout class="QVBoxLayout" name="verticalLayout_45">
+                       <item>
+                        <widget class="QLabel" name="label_167">
+                         <property name="text">
+                          <string>When laser scans are used, a 2D grid map can be created. When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item>
+                        <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
+                         <item row="1" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_resolution">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="minimum">
+                            <double>0.010000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.050000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_159">
+                           <property name="text">
+                            <string>Resolution (cell size)</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_164">
+                           <property name="text">
+                            <string>Fill empty space</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
+                           <property name="suffix">
+                            <string/>
+                           </property>
+                           <property name="minimum">
+                            <double>0.750000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.750000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_170">
+                           <property name="text">
+                            <string>Opacity</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="0">
+                          <widget class="QCheckBox" name="checkBox_map_shown">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_map_shown">
+                           <property name="text">
+                            <string>Show in 3D map view</string>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>

tools/DatabaseViewer/CMakeLists.txt

@@ -8,6 +8,7 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
 )
 
 INCLUDE(${QT_USE_FILE})
@@ -15,8 +16,11 @@ INCLUDE(${QT_USE_FILE})
 SET(LIBRARIES
 	${OpenCV_LIBRARIES}
 	${QT_LIBRARIES} 
+	${PCL_LIBRARIES}
 )
 
+add_definitions(${PCL_DEFINITIONS})
+
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 IF(MINGW)