From dd7d28898bd7eed34b7f8fe4a7b5a78c0d3d90c3 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 7 May 2015 15:32:56 -0400
Subject: [PATCH] Matching map colors to path/cloud/scan

---
 guilib/include/rtabmap/gui/CloudViewer.h |  2 +-
 guilib/src/CloudViewer.cpp               | 13 +++++++++----
 guilib/src/MainWindow.cpp                | 22 ++++++++--------------
 3 files changed, 18 insertions(+), 19 deletions(-)

diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h
index 4e77fbf9..133f84a5 100644
--- a/guilib/include/rtabmap/gui/CloudViewer.h
+++ b/guilib/include/rtabmap/gui/CloudViewer.h
@@ -224,7 +224,7 @@ private:
     QAction * _aSetGridCellSize;
     QAction * _aSetBackgroundColor;
     QMenu * _menu;
-    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr > _graphes;
+    std::set<std::string> _graphes;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
     unsigned int _gridCellCount;
diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp
index e0583137..be610ba4 100644
--- a/guilib/src/CloudViewer.cpp
+++ b/guilib/src/CloudViewer.cpp
@@ -552,7 +552,7 @@ void CloudViewer::addOrUpdateGraph(
 
 	if(graph->size())
 	{
-		_graphes.insert(std::make_pair(id, graph));
+		_graphes.insert(id);
 
 		pcl::PolygonMesh mesh;
 		pcl::Vertices vertices;
@@ -565,6 +565,9 @@ void CloudViewer::addOrUpdateGraph(
 		mesh.polygons.push_back(vertices);
 		_visualizer->addPolylineFromPolygonMesh(mesh, id);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
+
+		this->addOrUpdateCloud(id+"_nodes", graph, Transform::getIdentity(), color);
+		this->setCloudPointSize(id+"_nodes", 5);
 	}
 }
 
@@ -580,16 +583,18 @@ void CloudViewer::removeGraph(const std::string & id)
 	{
 		_visualizer->removeShape(id);
 		_graphes.erase(id);
+		removeCloud(id+"_nodes");
 	}
 }
 
 void CloudViewer::removeAllGraphs()
 {
-	for(std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = _graphes.begin(); iter!=_graphes.end(); ++iter)
+	std::set<std::string> graphes = _graphes;
+	for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
 	{
-		_visualizer->removeShape(iter->first);
+		this->removeGraph(*iter);
 	}
-	_graphes.clear();
+	UASSERT(_graphes.empty());
 }
 
 bool CloudViewer::isTrajectoryShown() const
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index 95c452fd..4210779d 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -1433,12 +1433,11 @@ void MainWindow::updateMapCloud(
 	{
 		// Find all graphs
 		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
-		pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
-		graphNodes->resize(_currentPosesMap.size());
-		int oi = 0;
 		for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
 		{
 			int mapId = uValue(_currentMapIds, iter->first, -1);
+
+			//edges
 			std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
 			if(kter == graphs.end())
 			{
@@ -1446,7 +1445,6 @@ void MainWindow::updateMapCloud(
 			}
 			pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
 			kter->second->push_back(pt);
-			(*graphNodes)[oi++] = pcl::PointXYZ(pt);
 		}
 
 		// add graphs
@@ -1455,14 +1453,10 @@ void MainWindow::updateMapCloud(
 			QColor color = Qt::gray;
 			if(iter->first >= 0)
 			{
-				color = (Qt::GlobalColor)((iter->first+2) % 12 + 7 );
+				color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
 			}
 			_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
-
-		// add nodes
-		_ui->widget_cloudViewer->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
-		_ui->widget_cloudViewer->setCloudPointSize("graph_nodes", 5);
 	}
 
 	// Update occupancy grid map in 3D map view and graph view
@@ -1649,7 +1643,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			QColor color = Qt::gray;
 			if(mapId >= 0)
 			{
-				color = (Qt::GlobalColor)(mapId % 12 + 7 );
+				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 			}
 			if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
 			{
@@ -1666,7 +1660,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		QColor color = Qt::gray;
 		if(mapId >= 0)
 		{
-			color = (Qt::GlobalColor)(mapId % 12 + 7 );
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 		}
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 		cloud->resize(iter->getWords3().size());
@@ -1721,12 +1715,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		cloud = util3d::laserScanToPointCloud(depth2D);
-		QColor color = Qt::red;
+		QColor color = Qt::gray;
 		if(mapId >= 0)
 		{
-			color = (Qt::GlobalColor)(mapId % 12 + 7 );
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 		}
-		if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
+		if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
 		{
 			UERROR("Adding cloud %d to viewer failed!", nodeId);
 		}