Graph viewer optimizations (#1616)

* DbViewer: coloring graph based on env sensor value (e.g., wifi signal strength)

* GraphViewer optimizations

* Fixed link memory allocation

* refactor

* more optimizations

* mainwindow keypoints optimization

* MainWindow optimizations

* Shrink graphics scene on clearAll

guilib/src/DatabaseViewer.cpp

@@ -394,11 +394,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
 	connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons()));
 
-	ui_->horizontalSlider_iterations->setTracking(false);
+	ui_->horizontalSlider_iterations->setTracking(true);
 	ui_->horizontalSlider_iterations->setEnabled(false);
 	ui_->spinBox_optimizationsFrom->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_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -7104,6 +7103,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 void DatabaseViewer::sliderIterationsValueChanged(int value)
 {
+	UDEBUG("sender=%s value=%d currentValue = %d", sender()?sender()->objectName().toStdString().c_str():"NA", value, ui_->horizontalSlider_iterations->value());
 	if(dbDriver_ && value >=0 && value < (int)graphes_.size())
 	{
 		std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
@@ -7284,6 +7284,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					break;
 			}
 			ui_->graphViewer->updateNodeColorByValue(legend, colors, 0.0f, 0.0f, invertedColor, 0, hueMax, 1);
+			UDEBUG("Updated node color based on env sensor %d", (int)curentType);
 		}
 		else if(ui_->checkBox_wmState->isEnabled() &&
 		   ui_->checkBox_wmState->isChecked() &&
@@ -7306,6 +7307,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			{
 				ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1);
 			}
+			UDEBUG("Updated node color based working memory state");
 		}
 		QGraphicsRectItem * rectScaleItem = 0;
 		ui_->graphViewer->clearMap();
@@ -7509,13 +7511,16 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}
 #endif
 		}
-		ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+		
 		if(rectScaleItem != 0)
 		{
 			ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
 			ui_->graphViewer->scene()->removeItem(rectScaleItem);
 			delete rectScaleItem;
 		}
+		else {
+			ui_->graphViewer->fitInView(ui_->graphViewer->sceneRect(), Qt::KeepAspectRatio);
+		}
 
 		ui_->graphViewer->update();
 		ui_->label_iterations->setNum(value);

guilib/src/GraphViewer.cpp

@@ -337,17 +337,37 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_gridCellSize(0.0f),
 		_localRadius(0),
 		_loopClosureOutlierThr(0),
-		_maxLinkLength(0.02f),
+		_minLinkLength(0.02f),
 		_orientationENU(false),
 		_mouseTracking(false),
 		_viewPlane(XY),
 		_ensureFrameVisible(true)
 {
-	this->setScene(new QGraphicsScene(this));
 	this->setDragMode(QGraphicsView::ScrollHandDrag);
 	_workingDirectory = QDir::homePath();
 
-	this->scene()->clear();
+	setupGraphicsScene();
+
+	// Match by default scan colors from DatabaseViewer
+	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::yellow, nullptr));
+	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::magenta, nullptr));
+
+	this->restoreDefaults();
+
+	this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
+}
+
+GraphViewer::~GraphViewer()
+{
+}
+
+void GraphViewer::setupGraphicsScene()
+{
+	if(this->scene())
+	{
+		delete this->scene();
+	}
+	this->setScene(new QGraphicsScene(this));
 	_world = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
 	_root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
 	_root->setParentItem(_world);
@@ -485,17 +505,6 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_odomCacheOverlay->setBrush(QBrush(QColor(255, 255, 255, 150)));
 	_odomCacheOverlay->setPen(QPen(Qt::NoPen));
 
-	// Match by default scan colors from DatabaseViewer
-	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::yellow, nullptr));
-	_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::magenta, nullptr));
-
-	this->restoreDefaults();
-
-	this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
-}
-
-GraphViewer::~GraphViewer()
-{
 }
 
 void GraphViewer::setWorldMapRotation(const float & theta)
@@ -510,18 +519,48 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::map<int, int> & weights,
 				 const std::set<int> & odomCacheIds)
 {
-	UTimer timer;
-	bool wasVisible = _graphRoot->isVisible();
-	_graphRoot->show();
-
-	bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
-	UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
-	//Hide nodes and links
-	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+	if(!_graphRoot->isVisible())
 	{
+		UDEBUG("Ignoring updating graph, the graph root is not visible.");
+		return;
+	}
+
+	UTimer timer;
+
+	bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0 && _gridMap->pixmap().isNull();
+	UDEBUG("poses=%ld constraints=%ld mapIds=%ld weights=%ld", poses.size(), constraints.size(), mapIds.size(), weights.size());
+	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+	UDEBUG("hidden %d links", _linkItems.size());
+
+	int created = 0;
+	int reused = 0;
+	int removed = 0;
+	QMap<int, NodeItem*>::iterator nter = _nodeItems.begin();
+	std::map<int, Transform>::const_iterator iter=_nodeVisible?poses.begin():poses.end();
+	while(nter!=_nodeItems.end() || iter!=poses.end())
+	{
+		if(nter!=_nodeItems.end() && (iter==poses.end() || nter.key() < iter->first || iter->second.isNull()))
+		{
+			// NodeItem is not in poses anymore, increase only _nodeItems iterator
+			for(int i=0; i<_highlightedNodes.size(); ++i)
+			{
+				if(_highlightedNodes[i].second && _highlightedNodes[i].second == nter.value())
+				{
+					_highlightedNodes[i].second = nullptr;
+				}
+			}
+			delete nter.value();
+			nter = _nodeItems.erase(nter);
+			++removed;
+			continue;
+		}
+
 		QColor color = _nodeColor;
-		bool isOdomCache = odomCacheIds.find(iter.key()) != odomCacheIds.end();
-		if(iter.key()<0)
+		bool isOdomCache = odomCacheIds.find(iter->first) != odomCacheIds.end();
+		if(iter->first<0)
 		{
 			color = QColor(255-color.red(), 255-color.green(), 255-color.blue());
 		}
@@ -529,51 +568,50 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		{
 			color = _nodeOdomCacheColor;
 		}
-		iter.value()->hide();
-		iter.value()->setColor(color); // reset color
-		iter.value()->setToolTipInfo(QString());
-		iter.value()->setZValue(iter.key()<0?21:20);
-	}
-	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
-	{
-		iter.value()->hide();
-	}
 
-	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-	{
-		if(!iter->second.isNull())
+		UASSERT(iter!=poses.end());
+
+		if(nter == _nodeItems.end() || nter.key() > iter->first)
 		{
-			QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
-			if(itemIter != _nodeItems.end())
-			{
-				itemIter.value()->setPose(iter->second, _viewPlane);
-				itemIter.value()->show();
-			}
-			else
-			{
-				// create node item
-				QColor color = _nodeColor;
-				bool isOdomCache = odomCacheIds.find(iter->first) != odomCacheIds.end();
-				if(iter->first<0)
-				{
-					color = QColor(255-color.red(), 255-color.green(), 255-color.blue());
-				}
-				else if(isOdomCache)
-				{
-					color = _nodeOdomCacheColor;
-				}
-				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth);
-				this->scene()->addItem(item);
-				item->setZValue(iter->first<0?21:20);
-				item->setColor(color);
-				item->setParentItem(_graphRoot);
-				item->show();
-				_nodeItems.insert(iter->first, item);
-			}
+			// NodeItem is not in poses, create a new one and increase poses iterator
+			const Transform & pose = iter->second;
+			NodeItem * item = new NodeItem(
+				iter->first, 
+				uValue(mapIds, iter->first, -1), 
+				pose, 
+				_nodeRadius,
+				uValue(weights, iter->first, -1), 
+				_viewPlane, 
+				_linkWidth);
+			this->scene()->addItem(item);
+			item->setZValue(iter->first<0?21:20);
+			item->setColor(color);
+			item->setParentItem(_graphRoot);
+			item->show();
+			_nodeItems.insert(iter->first, item);
+			++iter;
+			++created;
+		}
+		else
+		{
+			// NodeItem exists for the pose, copy data and increase both iterators
+			UASSERT(iter->first == nter.key());
+			nter.value()->setColor(color); // reset color
+			nter.value()->setToolTipInfo(QString());
+			nter.value()->setZValue(iter->first<0?21:20);
+			nter.value()->setPose(iter->second, _viewPlane);
+			nter.value()->show();
+			++nter;
+			++iter;
+			++reused;
 		}
 	}
-
+	UDEBUG("Nodes created=%d, reused=%d removed=%d", created, reused, removed);
+	created = 0;
+	reused = 0;
+	removed = 0;
+	int removedSmallLinks = 0;
+	int ignoredSmallLinks = 0;
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		// make the first id the smallest one
@@ -587,30 +625,28 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
 		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		LinkItem * linkItem = 0;
-		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
+		if(jterA != poses.end() && jterB != poses.end())
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
 
-			QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
-
-			if(_linkItems.contains(idFrom))
+			QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
+			bool alreadyAdded = false;
+			while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
 			{
-				itemIter = _linkItems.find(idFrom);
-				bool alreadyAdded = false;
-				while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
+				if(itemIter.value()->to() == idTo)
 				{
-					if(itemIter.value()->to() == idTo && itemIter.value()->isVisible())
-					{
+					if(itemIter.value()->isVisible()) {
 						alreadyAdded = true;
-						break;
+					} else {
+						linkItem = itemIter.value();
 					}
-					++itemIter;
-				}
-				if(alreadyAdded){
-					continue;
+					break;
 				}
+				++itemIter;
+			}
+			if(alreadyAdded){
+				continue;
 			}
 
 			bool interSessionClosure = false;
@@ -625,11 +661,15 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 
 			if(isLinkedToOdomCachePoses)
 			{
-				_nodeItems.value(idFrom)->setZValue(odomCacheIds.find(idFrom)!=odomCacheIds.end()?24:23);
-				_nodeItems.value(idTo)->setZValue(odomCacheIds.find(idTo)!=odomCacheIds.end()?24:23);
+				if(_nodeItems.contains(idFrom)) {
+					_nodeItems.value(idFrom)->setZValue(odomCacheIds.find(idFrom)!=odomCacheIds.end()?24:23);
+				}
+				if(_nodeItems.contains(idTo)) {
+					_nodeItems.value(idTo)->setZValue(odomCacheIds.find(idTo)!=odomCacheIds.end()?24:23);
+				}
 			}
 
-			if(poseA.getDistance(poseB) > _maxLinkLength)
+			if(poseA.getDistance(poseB) > _minLinkLength)
 			{
 				if(linkItem == 0)
 				{
@@ -642,14 +682,24 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 					this->scene()->addItem(linkItem);
 					linkItem->setParentItem(_graphRoot);
 					_linkItems.insert(idFrom, linkItem);
+					++created;
+				}
+				else {
+					linkItem->setPoses(poseA, poseB, _viewPlane);
+					linkItem->show();
+					++reused;
 				}
 			}
-			else if(linkItem && itemIter != _linkItems.end())
+			else if(linkItem)
 			{
 				// erase small links
 				_linkItems.erase(itemIter);
 				delete linkItem;
 				linkItem = 0;
+				++removedSmallLinks;
+			}
+			else {
+				++ignoredSmallLinks;
 			}
 
 			if(linkItem)
@@ -730,71 +780,48 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			}
 		}
 	}
-
+	UDEBUG("Links created=%d, reused=%d, small links: removed=%d ignored=%d", created, reused, removedSmallLinks, ignoredSmallLinks);
 	//remove not used nodes and links
-	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end();)
-	{
-		if(!iter.value()->isVisible())
-		{
-			for(int i=0; i<_highlightedNodes.size(); ++i)
-			{
-				if(_highlightedNodes[i].second && _highlightedNodes[i].second == iter.value())
-				{
-					_highlightedNodes[i].second = nullptr;
-				}
-			}
-
-			delete iter.value();
-			iter = _nodeItems.erase(iter);
-		}
-		else
-		{
-			iter.value()->setVisible(_nodeVisible);
-			++iter;
-		}
-	}
+	removed = 0;
+	int visible = 0;
 	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end();)
 	{
 		if(!iter.value()->isVisible())
 		{
 			delete iter.value();
 			iter = _linkItems.erase(iter);
+			++removed;
 		}
 		else
 		{
 			++iter;
+			++visible;
 		}
 	}
-
+	UDEBUG("Links removed=%d, visible=%d", removed, visible);
 	if(_nodeItems.size())
 	{
 		(--_nodeItems.end()).value()->setColor(_nodeOdomCacheColor);
 	}
-
-	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-
-	if(!odomCacheIds.empty())
-		_odomCacheOverlay->setRect(this->scene()->itemsBoundingRect());
-	else
-		_odomCacheOverlay->setRect(0, 0, 0, 0);
-
+	
 	if(wasEmpty)
 	{
-		QRectF rect = this->scene()->itemsBoundingRect();
+		QRectF rect = this->scene()->sceneRect();
 		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
 	}
 
-	_graphRoot->setVisible(wasVisible);
-
 	UDEBUG("_nodeItems=%d, _linkItems=%d, timer=%fs", _nodeItems.size(), _linkItems.size(), timer.ticks());
 }
 
 void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 {
+	if(!_gtGraphRoot->isVisible())
+	{
+		UDEBUG("Ignoring updating gt graph, the graph root is not visible.");
+		return;
+	}
+
 	UTimer timer;
-	bool wasVisible = _gtGraphRoot->isVisible();
-	_gtGraphRoot->show();
-	bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
 	UDEBUG("poses=%d", (int)poses.size());
 	//Hide nodes and links
 	for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
@@ -811,23 +838,26 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 	{
 		if(!iter->second.isNull())
 		{
-			QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
-			if(itemIter != _gtNodeItems.end())
+			if(_nodeVisible)
 			{
-				itemIter.value()->setPose(iter->second, _viewPlane);
-				itemIter.value()->show();
-			}
-			else
-			{
-				// create node item
-				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth);
-				this->scene()->addItem(item);
-				item->setZValue(20);
-				item->setColor(_gtPathColor);
-				item->setParentItem(_gtGraphRoot);
-				item->setVisible(_nodeVisible);
-				_gtNodeItems.insert(iter->first, item);
+				QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
+				if(itemIter != _gtNodeItems.end())
+				{
+					itemIter.value()->setPose(iter->second, _viewPlane);
+					itemIter.value()->show();
+				}
+				else
+				{
+					// create node item
+					const Transform & pose = iter->second;
+					NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth);
+					this->scene()->addItem(item);
+					item->setZValue(20);
+					item->setColor(_gtPathColor);
+					item->setParentItem(_gtGraphRoot);
+					item->setVisible(_nodeVisible);
+					_gtNodeItems.insert(iter->first, item);
+				}
 			}
 
 			if(iter!=poses.begin())
@@ -913,20 +943,6 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 			++iter;
 		}
 	}
-
-	if(_gtNodeItems.size() || _gtLinkItems.size())
-	{
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-
-		if(wasEmpty)
-		{
-			QRectF rect = this->scene()->itemsBoundingRect();
-			this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
-		}
-	}
-
-	_gtGraphRoot->setVisible(wasVisible);
-
 	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
 }
 
@@ -934,10 +950,11 @@ void GraphViewer::updateGPSGraph(
 		const std::map<int, Transform> & poses,
 		const std::map<int, GPS> & gpsValues)
 {
+	if(!_gpsGraphRoot->isVisible()) {
+		UDEBUG("Ignoring updating gps graph, the graph root is not visible.");
+		return;
+	}
 	UTimer timer;
-	bool wasVisible = _gpsGraphRoot->isVisible();
-	_gpsGraphRoot->show();
-	bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0;
 	UDEBUG("poses=%d", (int)poses.size());
 	//Hide nodes and links
 	for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
@@ -954,24 +971,27 @@ void GraphViewer::updateGPSGraph(
 	{
 		if(!iter->second.isNull())
 		{
-			QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
-			if(itemIter != _gpsNodeItems.end())
+			if(_nodeVisible)
 			{
-				itemIter.value()->setPose(iter->second, _viewPlane);
-				itemIter.value()->show();
-			}
-			else
-			{
-				// create node item
-				const Transform & pose = iter->second;
-				UASSERT(gpsValues.find(iter->first) != gpsValues.end());
-				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth);
-				this->scene()->addItem(item);
-				item->setZValue(20);
-				item->setColor(_gpsPathColor);
-				item->setParentItem(_gpsGraphRoot);
-				item->setVisible(_nodeVisible);
-				_gpsNodeItems.insert(iter->first, item);
+				QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
+				if(itemIter != _gpsNodeItems.end())
+				{
+					itemIter.value()->setPose(iter->second, _viewPlane);
+					itemIter.value()->show();
+				}
+				else
+				{
+					// create node item
+					const Transform & pose = iter->second;
+					UASSERT(gpsValues.find(iter->first) != gpsValues.end());
+					NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth);
+					this->scene()->addItem(item);
+					item->setZValue(20);
+					item->setColor(_gpsPathColor);
+					item->setParentItem(_gpsGraphRoot);
+					item->setVisible(_nodeVisible);
+					_gpsNodeItems.insert(iter->first, item);
+				}
 			}
 
 			if(iter!=poses.begin())
@@ -1043,20 +1063,6 @@ void GraphViewer::updateGPSGraph(
 			++iter;
 		}
 	}
-
-	if(_gpsNodeItems.size() || _gpsLinkItems.size())
-	{
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-
-		if(wasEmpty)
-		{
-			QRectF rect = this->scene()->itemsBoundingRect();
-			this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
-		}
-	}
-
-	_gpsGraphRoot->setVisible(wasVisible);
-
 	UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks());
 }
 
@@ -1085,6 +1091,7 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 	UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f));
 	if(!map8U.empty())
 	{
+		bool wasEmpty = _nodeItems.size() <= 1 && _linkItems.size() == 0 && _gridMap->pixmap().isNull();
 		_gridCellSize = resolution;
 		QImage image = uCvMat2QImage(map8U, false);
 		_gridMap->resetTransform();
@@ -1092,8 +1099,11 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 		_gridMap->setRotation(90);
 		_gridMap->setPixmap(QPixmap::fromImage(image));
 		_gridMap->setPos(-yMin*100.0f, -xMin*100.0f);
-		// Re-shrink the scene to it's bounding contents
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
+
+		if(wasEmpty)
+		{
+			this->fitInView(this->scene()->sceneRect(), Qt::KeepAspectRatio);
+		}
 	}
 	else
 	{
@@ -1186,10 +1196,10 @@ void GraphViewer::updateNodeColorByValue(
 			}
 		}
 	}
-	else if(min < max) {
+	else if(min >= max) {
 		UWARN("min (%f) is not less than max (%f), cannot change color of the graph.", min, max);
 	}
-	else if(hueMin < hueMax) {
+	else if(hueMin >= hueMax) {
 		UWARN("Hue min (%d) is not less than hue max (%d), cannot change color of the graph. The hue values should be set between 0 (red) and 360(pink).", (int)hueMin, (int)hueMax);
 	}
 }
@@ -1260,6 +1270,11 @@ void GraphViewer::setNodeInfo(int id, const QString & info)
 void GraphViewer::setLocalRadius(float radius)
 {
 	_localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);
+	if(_nodeItems.empty() && _linkItems.empty() && _gridMap->pixmap().isNull())
+	{
+		QRectF rect = this->scene()->sceneRect();
+		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
+	}
 }
 
 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
@@ -1384,14 +1399,17 @@ void GraphViewer::clearGraph()
 	_worldMapRotation = 0.0f;
 	_referential->resetTransform();
 	_localRadius->resetTransform();
-	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
 }
 
 void GraphViewer::clearMap()
 {
-	_gridMap->setPixmap(QPixmap());
 	_gridCellSize = 0.0f;
-	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+	if(_gridMap->pixmap().isNull())
+	{
+		// there is no grid map, just return
+		return;
+	}
+	_gridMap->setPixmap(QPixmap());
 }
 
 void GraphViewer::clearPosterior()
@@ -1411,6 +1429,14 @@ void GraphViewer::clearAll()
 {
 	clearMap();
 	clearGraph();
+
+	// The only way to re-shrink the dynamic scene rect is to re-create the QGraphicsScene.
+	QSettings tmp;
+	saveSettings(tmp);
+	QRectF localRadiusRectF = _localRadius->rect();
+	setupGraphicsScene(); 
+	loadSettings(tmp); // restore previous state
+	_localRadius->setRect(localRadiusRectF);
 }
 
 void GraphViewer::saveSettings(QSettings & settings, const QString & group) const
@@ -1447,8 +1473,9 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("referential_visible", this->isReferentialVisible());
 	settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
 	settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr());
-	settings.setValue("max_link_length", this->getMaxLinkLength());
+	settings.setValue("min_link_length", this->getMinLinkLength());
 	settings.setValue("graph_visible", this->isGraphVisible());
+	settings.setValue("node_visible", this->isNodeVisible());
 	settings.setValue("global_path_visible", this->isGlobalPathVisible());
 	settings.setValue("local_path_visible", this->isLocalPathVisible());
 	settings.setValue("gt_graph_visible", this->isGtGraphVisible());
@@ -1497,8 +1524,9 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool());
 	this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool());
 	this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble());
-	this->setMaxLinkLength(settings.value("max_link_length", this->getMaxLinkLength()).toDouble());
+	this->setMinLinkLength(settings.value("min_link_length", this->getMinLinkLength()).toDouble());
 	this->setGraphVisible(settings.value("graph_visible", this->isGraphVisible()).toBool());
+	this->setNodeVisible(settings.value("node_visible", this->isNodeVisible()).toBool());
 	this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
 	this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
 	this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
@@ -1533,6 +1561,10 @@ bool GraphViewer::isGraphVisible() const
 {
 	return _graphRoot->isVisible();
 }
+bool GraphViewer::isNodeVisible() const
+{
+	return _nodeVisible;
+}
 bool GraphViewer::isGlobalPathVisible() const
 {
 	return _globalPathRoot->isVisible();
@@ -1855,9 +1887,9 @@ void GraphViewer::setLoopClosureOutlierThr(float value)
 {
 	_loopClosureOutlierThr = value;
 }
-void GraphViewer::setMaxLinkLength(float value)
+void GraphViewer::setMinLinkLength(float value)
 {
-	_maxLinkLength = value;
+	_minLinkLength = value;
 }
 void GraphViewer::setGraphVisible(bool visible)
 {
@@ -1898,10 +1930,6 @@ void GraphViewer::setOrientationENU(bool enabled)
 	QTransform t;
 	t.rotateRadians(_worldMapRotation);
 	_root->setTransform(t);
-	if(_nodeItems.size() || _linkItems.size())
-	{
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-	}
 }
 
 void GraphViewer::setViewPlane(ViewPlane plane)
@@ -1928,11 +1956,6 @@ void GraphViewer::setViewPlane(ViewPlane plane)
 	_referentialXY->setVisible(plane==XY);
 	_referentialXZ->setVisible(plane==XZ);
 	_referentialYZ->setVisible(plane==YZ);
-
-	if(_nodeItems.size() || _linkItems.size())
-	{
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-	}
 }
 void GraphViewer::setEnsureFrameVisible(bool visible)
 {
@@ -2123,7 +2146,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	menu.addSeparator();
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
 	QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
-	QAction * aChangeMaxLinkLength = menu.addAction(tr("Set maximum link length..."));
+	QAction * aChangeMinLinkLength = menu.addAction(tr("Set minimum link length..."));
 	menu.addSeparator();
 	QAction * aEnsureFrameVisible;
 	QAction * aShowHideGridMap;
@@ -2241,12 +2264,12 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aMouseTracking->setCheckable(true);
 	aMouseTracking->setChecked(_mouseTracking);
 	aMouseTracking->setEnabled(_viewPlane == XY);
-	aShowHideGraph->setEnabled(_nodeItems.size() && _viewPlane == XY);
-	aShowHideGraphNodes->setEnabled(_nodeItems.size() && _graphRoot->isVisible());
+	aShowHideGraph->setEnabled(_viewPlane == XY);
+	aShowHideGraphNodes->setEnabled(_graphRoot->isVisible());
 	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
 	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
-	aShowHideGtGraph->setEnabled(_gtNodeItems.size());
-	aShowHideGPSGraph->setEnabled(_gpsNodeItems.size());
+	aShowHideGtGraph->setEnabled(_gtGraphRoot->isVisible());
+	aShowHideGPSGraph->setEnabled(_gpsGraphRoot->isVisible());
 	aShowHideOdomCacheOverlay->setEnabled(_odomCacheOverlay->rect().width()>0);
 
 	QMenu * viewPlaneMenu = menu.addMenu("View Plane...");
@@ -2363,8 +2386,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 
 				//reset scale
 				_root->setScale(1.0f);
-				this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-
+				this->scene()->setSceneRect(QRectF());
 
 				QDesktopServices::openUrl(QUrl::fromLocalFile(filePath));
 			}
@@ -2426,13 +2448,13 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			setLoopClosureOutlierThr(value);
 		}
 	}
-	else if(r == aChangeMaxLinkLength)
+	else if(r == aChangeMinLinkLength)
 	{
 		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Maximum link length to be shown"), tr("Value (m)"), _maxLinkLength, 0.0, 1000.0, 3, &ok);
+		double value = QInputDialog::getDouble(this, tr("Minimum link length to be shown"), tr("Value (m)"), _minLinkLength, 0.0, 1000.0, 3, &ok);
 		if(ok)
 		{
-			setMaxLinkLength(value);
+			setMinLinkLength(value);
 		}
 	}
 	else if(r == aChangeNodeColor ||

guilib/src/MainWindow.cpp

@@ -1588,63 +1588,61 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	{
 		_odometryReceived = true;
 		// update camera position
-		if(data->cameraModels().size() && data->cameraModels()[0].isValidForProjection())
+		if(_cloudViewer->isVisible())
 		{
-			_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels());
-		}
-		else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection())
-		{
-			_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels());
-		}
-		else if(!data->laserScanRaw().isEmpty() ||
-				!data->laserScanCompressed().isEmpty())
-		{
-			Transform scanLocalTransform;
-			if(!data->laserScanRaw().isEmpty())
+			if(data->cameraModels().size() && data->cameraModels()[0].isValidForProjection())
 			{
-				scanLocalTransform = data->laserScanRaw().localTransform();
+				_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels());
+			}
+			else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection())
+			{
+				_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels());
+			}
+			else if(!data->laserScanRaw().isEmpty() ||
+					!data->laserScanCompressed().isEmpty())
+			{
+				Transform scanLocalTransform;
+				if(!data->laserScanRaw().isEmpty())
+				{
+					scanLocalTransform = data->laserScanRaw().localTransform();
+				}
+				else
+				{
+					scanLocalTransform = data->laserScanCompressed().localTransform();
+				}
+				//fake frustum
+				CameraModel model(
+						2,
+						2,
+						2,
+						1.5,
+						scanLocalTransform*CameraModel::opticalRotation(),
+						0,
+						cv::Size(4,3));
+				_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model);
+
+			}
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			if(_preferencesDialog->isFramesShown())
+			{
+				_cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false);
 			}
 			else
 			{
-				scanLocalTransform = data->laserScanCompressed().localTransform();
+				_cloudViewer->removeLine("odom_to_base_link");
 			}
-			//fake frustum
-			CameraModel model(
-					2,
-					2,
-					2,
-					1.5,
-					scanLocalTransform*CameraModel::opticalRotation(),
-					0,
-					cv::Size(4,3));
-			_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model);
-
-		}
-#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-		if(_preferencesDialog->isFramesShown())
-		{
-			_cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false);
-		}
-		else
-		{
-			_cloudViewer->removeLine("odom_to_base_link");
-		}
 #endif
-		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
-		UDEBUG("Time Update Pose: %fs", time.ticks());
-	}
-
-	_cloudViewer->refreshView();
-
-	if(_ui->graphicsView_graphView->isVisible())
-	{
-		if(!pose.isNull() && !odom.pose().isNull())
+			_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
+			UDEBUG("Time Update Pose: %fs", time.ticks());
+			_cloudViewer->refreshView();
+		}
+		if(_ui->graphicsView_graphView->isVisible())
 		{
 			_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
 			_ui->graphicsView_graphView->update();
 			UDEBUG("Time Update graphview: %fs", time.ticks());
 		}
-	}
+	}	
 
 	if(_ui->dockWidget_odometry->isVisible() &&
 	   !data->imageRaw().empty())
@@ -2031,13 +2029,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			// make sure data are uncompressed
 			// We don't need to uncompress images if we don't show them
-			bool uncompressImages = !signature.sensorData().imageCompressed().empty() && (
-					_ui->imageView_source->isVisible() ||
-					(_loopClosureViewer->isVisible() &&
-							!signature.sensorData().depthOrRightCompressed().empty()) ||
-					(_cloudViewer->isVisible() &&
-							_preferencesDialog->isCloudsShown(0) &&
-							!signature.sensorData().depthOrRightCompressed().empty()));
+			bool uncompressImages = (!signature.sensorData().imageCompressed().empty() && 
+										((_ui->imageView_source->isVisible() && _ui->imageView_source->isImageShown()) ||
+										 _loopClosureViewer->isVisible()))
+									||
+									(!signature.sensorData().depthOrRightCompressed().empty() && 
+									 ((_ui->imageView_loopClosure->isVisible() && _ui->imageView_loopClosure->isImageShown()) ||
+									  (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0))));
+
 			bool uncompressScan = !signature.sensorData().laserScanCompressed().isEmpty() && (
 					_loopClosureViewer->isVisible() ||
 					(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0)));
@@ -2116,11 +2115,19 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			_ui->imageView_source->clear();
 			_ui->imageView_loopClosure->clear();
 
-			if(signature.sensorData().imageRaw().empty() && signature.getWords().empty())
+			// To see colors
+			QRect rect(0,0,640,480); // default
+			if(signature.sensorData().cameraModels().size() && signature.sensorData().cameraModels().at(0).imageSize()!=cv::Size())
 			{
-				// To see colors
-				_ui->imageView_source->setSceneRect(QRect(0,0,640,480));
+				rect.setWidth(signature.sensorData().cameraModels().at(0).imageWidth()*signature.sensorData().cameraModels().size());
+				rect.setHeight(signature.sensorData().cameraModels().at(0).imageHeight());
 			}
+			else if(signature.sensorData().stereoCameraModels().size() && signature.sensorData().stereoCameraModels().at(0).left().imageSize()!=cv::Size())
+			{
+				rect.setWidth(signature.sensorData().stereoCameraModels().at(0).left().imageWidth()*signature.sensorData().stereoCameraModels().size());
+				rect.setHeight(signature.sensorData().stereoCameraModels().at(0).left().imageHeight());
+			}
+			_ui->imageView_source->setSceneRect(rect);
 
 			_ui->imageView_source->setBackgroundColor(_ui->imageView_source->getDefaultBackgroundColor());
 			_ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
@@ -2261,22 +2268,27 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
 					if(iter != _cachedSignatures.end())
 					{
-						// uncompress after copy to avoid keeping uncompressed data in memory
 						loopSignature = iter.value();
-						bool uncompressImages = !loopSignature.sensorData().imageCompressed().empty() && (
-								_ui->imageView_source->isVisible() ||
-								(_loopClosureViewer->isVisible() &&
-										!loopSignature.sensorData().depthOrRightCompressed().empty()));
-						bool uncompressScan = _loopClosureViewer->isVisible() &&
-								!loopSignature.sensorData().laserScanCompressed().isEmpty();
-						if(uncompressImages || uncompressScan)
+
+						if((_ui->imageView_loopClosure->isVisible() && (_ui->imageView_loopClosure->isImageShown() || _ui->imageView_loopClosure->isImageDepthShown())) ||
+							_loopClosureViewer->isVisible())
 						{
-							cv::Mat tmpRGB, tmpDepth;
-							LaserScan tmpScan;
-							loopSignature.sensorData().uncompressData(
-									uncompressImages?&tmpRGB:0,
-									uncompressImages?&tmpDepth:0,
-									uncompressScan?&tmpScan:0);
+							// uncompress after copy to avoid keeping uncompressed data in memory
+							bool uncompressImages = !loopSignature.sensorData().imageCompressed().empty() && (
+									(_ui->imageView_loopClosure->isVisible() && (_ui->imageView_loopClosure->isImageShown() || _ui->imageView_loopClosure->isImageDepthShown())) ||
+									(_loopClosureViewer->isVisible() &&
+											!loopSignature.sensorData().depthOrRightCompressed().empty()));
+							bool uncompressScan = _loopClosureViewer->isVisible() &&
+									!loopSignature.sensorData().laserScanCompressed().isEmpty();
+							if(uncompressImages || uncompressScan)
+							{
+								cv::Mat tmpRGB, tmpDepth;
+								LaserScan tmpScan;
+								loopSignature.sensorData().uncompressData(
+										uncompressImages?&tmpRGB:0,
+										uncompressImages?&tmpDepth:0,
+										uncompressScan?&tmpScan:0);
+							}
 						}
 					}
 				}
@@ -2294,14 +2306,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			   !loopSignature.sensorData().imageRaw().empty() ||
 			   signature.getWords().size())
 			{
-				cv::Mat refImage = signature.sensorData().imageRaw();
-				cv::Mat loopImage = loopSignature.sensorData().imageRaw();
+				cv::Mat refImage = _ui->imageView_source->isImageShown()?signature.sensorData().imageRaw():cv::Mat();
+				cv::Mat loopImage =  _ui->imageView_loopClosure->isImageShown()?loopSignature.sensorData().imageRaw():cv::Mat();
 
 				if( _preferencesDialog->isMarkerDetection() &&
 					_preferencesDialog->isLandmarksShown())
 				{
 					//draw markers
-					if(!signature.getLandmarks().empty())
+					if(!signature.getLandmarks().empty() && !refImage.empty())
 					{
 						if(refImage.channels() == 1)
 						{
@@ -2315,7 +2327,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						}
 						drawLandmarks(refImage, signature);
 					}
-					if(!loopSignature.getLandmarks().empty())
+					if(!loopSignature.getLandmarks().empty() && !loopImage.empty())
 					{
 						if(loopImage.channels() == 1)
 						{
@@ -2345,42 +2357,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				{
 					_ui->imageView_source->setImage(img);
 				}
-				if(!signature.sensorData().depthOrRightRaw().empty())
+				if(!signature.sensorData().depthOrRightRaw().empty() && _ui->imageView_source->isImageDepthShown())
 				{
 					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw(), signature.sensorData().depthConfidenceRaw());
 				}
-				if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
-				{
-					QRect sceneRect;
-					if(signature.sensorData().cameraModels().size())
-					{
-						for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
-						{
-							sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
-							sceneRect.setHeight(std::max((int)sceneRect.height(), signature.sensorData().cameraModels()[i].imageHeight()));
-						}
-					}
-					else if(signature.sensorData().stereoCameraModels().size())
-					{
-						for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
-						{
-							sceneRect.setWidth(sceneRect.width()+signature.sensorData().stereoCameraModels()[i].left().imageWidth());
-							sceneRect.setHeight(std::max((int)sceneRect.height(), signature.sensorData().stereoCameraModels()[i].left().imageHeight()));
-						}
-					}
-					if(sceneRect.isValid())
-					{
-						_ui->imageView_source->setSceneRect(sceneRect);
-					}
-				}
 				if(!lcImg.isNull())
 				{
 					_ui->imageView_loopClosure->setImage(lcImg);
 				}
-				if(!loopSignature.sensorData().depthOrRightRaw().empty())
+				if(!loopSignature.sensorData().depthOrRightRaw().empty() && _ui->imageView_loopClosure->isImageDepthShown())
 				{
 					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw(), loopSignature.sensorData().depthConfidenceRaw());
 				}
+
+				if(lcImg.isNull())
+				{
+					QRect sceneRect;
+					if(loopSignature.sensorData().cameraModels().size() && loopSignature.sensorData().cameraModels().at(0).imageSize()!=cv::Size())
+					{
+						rect.setWidth(loopSignature.sensorData().cameraModels().at(0).imageWidth()*loopSignature.sensorData().cameraModels().size());
+						rect.setHeight(loopSignature.sensorData().cameraModels().at(0).imageHeight());
+					}
+					else if(loopSignature.sensorData().stereoCameraModels().size() && loopSignature.sensorData().stereoCameraModels().at(0).left().imageSize()!=cv::Size())
+					{
+						rect.setWidth(loopSignature.sensorData().stereoCameraModels().at(0).left().imageWidth()*loopSignature.sensorData().stereoCameraModels().size());
+						rect.setHeight(loopSignature.sensorData().stereoCameraModels().at(0).left().imageHeight());
+					}
+					if(sceneRect.isValid())
+					{
+						_ui->imageView_loopClosure->setSceneRect(sceneRect);
+					}
+				}
 				if(_ui->imageView_loopClosure->sceneRect().isNull())
 				{
 					_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
@@ -2394,24 +2401,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			UDEBUG("time= %d ms (update detection imageviews)", time.restart());
 
-			// do it after scaling
-			std::multimap<int, cv::KeyPoint> wordsA;
-			std::multimap<int, cv::KeyPoint> wordsB;
-			if(signature.getWords().size() == signature.getWordsKpts().size())
+			if(_ui->imageView_source->isFeaturesShown() || _ui->imageView_loopClosure->isFeaturesShown() || 
+			   (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown()))
 			{
-				for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
+				// do it after scaling
+				std::multimap<int, cv::KeyPoint> wordsA;
+				std::multimap<int, cv::KeyPoint> wordsB;
+				if(signature.getWords().size() == signature.getWordsKpts().size() &&
+				   (_ui->imageView_source->isFeaturesShown() || (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown())))
 				{
-					wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
+					for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
+					{
+						wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
+					}
 				}
-			}
-			if(loopSignature.getWords().size() == loopSignature.getWordsKpts().size())
-			{
-				for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
+				if(loopSignature.getWords().size() == loopSignature.getWordsKpts().size() && 
+				   (_ui->imageView_loopClosure->isFeaturesShown() || (_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown())))
 				{
-					wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
+					for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
+					{
+						wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
+					}
 				}
+				this->drawKeypoints(wordsA, wordsB);
+			}
+			else {
+				_ui->imageView_source->clearFeatures();
+				_ui->imageView_loopClosure->clearFeatures();
+				_ui->imageView_source->clearLines();
+				_ui->imageView_loopClosure->clearLines();
+				_lastIds.clear();
 			}
-			this->drawKeypoints(wordsA, wordsB);
 
 			UDEBUG("time= %d ms (draw keypoints)", time.restart());
 
@@ -2520,43 +2540,45 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId());
 			if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
 			{
-				if(poses.rbegin()->first == stat.getLastSignatureData().id())
+				if(_cloudViewer->isVisible())
 				{
-					if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
+					if(poses.rbegin()->first == stat.getLastSignatureData().id())
 					{
-						_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
-					}
-					else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection())
-					{
-						_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels());
-					}
-					else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() ||
-							!stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty())
-					{
-						Transform scanLocalTransform;
-						if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty())
+						if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
 						{
-							scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform();
+							_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
 						}
-						else
+						else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection())
 						{
-							scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform();
+							_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels());
+						}
+						else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() ||
+								!stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty())
+						{
+							Transform scanLocalTransform;
+							if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty())
+							{
+								scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform();
+							}
+							else
+							{
+								scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform();
+							}
+							//fake frustum
+							CameraModel model(
+									2,
+									2,
+									2,
+									1.5,
+									scanLocalTransform*CameraModel::opticalRotation(),
+									0,
+									cv::Size(4,3));
+							_cloudViewer->updateCameraFrustum(poses.rbegin()->second, model);
 						}
-						//fake frustum
-						CameraModel model(
-								2,
-								2,
-								2,
-								1.5,
-								scanLocalTransform*CameraModel::opticalRotation(),
-								0,
-								cv::Size(4,3));
-						_cloudViewer->updateCameraFrustum(poses.rbegin()->second, model);
 					}
+					_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
 				}
 
-				_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
-
 				if(_ui->graphicsView_graphView->isVisible())
 				{
 					_ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
@@ -2851,6 +2873,7 @@ void MainWindow::updateMapCloud(
 			_progressDialog->appendText(tr("Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(nodePoses.size()));
 			QApplication::processEvents();
 		}
+		UDEBUG("Filtered poses");
 	}
 	else
 	{
@@ -2858,27 +2881,30 @@ void MainWindow::updateMapCloud(
 		mapIds = mapIdsIn;
 	}
 
-	std::map<int, bool> posesMask;
-	for(std::map<int, Transform>::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter)
+	if(_ui->widget_mapVisibility->isVisible())
 	{
-		posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
+		std::map<int, bool> posesMask;
+		for(std::map<int, Transform>::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter)
+		{
+			posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
+		}
+		_ui->widget_mapVisibility->setMap(nodePoses, posesMask);
+		UDEBUG("Updated map visibility with %ld poses", nodePoses.size());
 	}
-	_ui->widget_mapVisibility->setMap(nodePoses, posesMask);
 
 	if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
 	{
+		int anchored = 0;
 		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
 			if(gtIter!=groundTruths.end())
 			{
 				iter->second = gtIter->second;
-			}
-			else
-			{
-				UWARN("Not found ground truth pose for node %d", iter->first);
+				++anchored;
 			}
 		}
+		UDEBUG("Anchored %d/%ld poses to ground truth", anchored, poses.size());
 	}
 	else if(_currentGTPosesMap.size() == 0)
 	{
@@ -3050,7 +3076,9 @@ void MainWindow::updateMapCloud(
 					cv::Mat obstacles;
 					cv::Mat empty;
 
+					UTimer decompressionTime;
 					jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
+					UDEBUG("Uncompressed local occupancy grid of node %d (%f s)", jter->id(), decompressionTime.ticks());
 
 					double resolution = jter->sensorData().gridCellSize();
 					if(_preferencesDialog->getGridUIResolution() > jter->sensorData().gridCellSize())
@@ -3219,6 +3247,7 @@ void MainWindow::updateMapCloud(
 	if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
 	{
 		mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
+		UDEBUG("Aligned poses to ground truth (%ld poses %ld gt poses)", _currentPosesMap.size(), _currentGTPosesMap.size());
 	}
 
 	std::map<int, Transform> posesWithOdomCache;
@@ -3233,7 +3262,9 @@ void MainWindow::updateMapCloud(
 		}
 	}
 
-	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
+	if( _cloudViewer->isVisible() && 
+		(_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && 
+		_currentPosesMap.size())
 	{
 		UTimer timerGraph;
 		// Find all graphs
@@ -3336,7 +3367,7 @@ void MainWindow::updateMapCloud(
 			}
 		}
 
-		UDEBUG("timerGraph=%fs", timerGraph.ticks());
+		UDEBUG("timerGraph (CloudViewer)=%fs", timerGraph.ticks());
 	}
 
 	UDEBUG("labels.size()=%d", (int)labels.size());
@@ -5021,96 +5052,113 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 	timer.start();
 	ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
-	if(refWords.size())
+	_ui->imageView_source->clearFeatures();
+	if(_ui->imageView_source->isFeaturesShown())
 	{
-		_ui->imageView_source->clearFeatures();
+		for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
+		{
+			int id = iter->first;
+			QColor color;
+			if(id<0)
+			{
+				// GRAY = NOT QUANTIZED
+				color = Qt::gray;
+			}
+			else if(uContains(loopWords, id))
+			{
+				// PINK = FOUND IN LOOP SIGNATURE
+				color = Qt::magenta;
+			}
+			else if(_lastIds.contains(id))
+			{
+				// BLUE = FOUND IN LAST SIGNATURE
+				color = Qt::blue;
+			}
+			else if(id<=_lastId)
+			{
+				// RED = ALREADY EXISTS
+				color = Qt::red;
+			}
+			else if(refWords.count(id) > 1)
+			{
+				// YELLOW = NEW and multiple times
+				color = Qt::yellow;
+			}
+			else
+			{
+				// GREEN = NEW
+				color = Qt::green;
+			}
+			_ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
+		}
 	}
-	for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
-	{
-		int id = iter->first;
-		QColor color;
-		if(id<0)
-		{
-			// GRAY = NOT QUANTIZED
-			color = Qt::gray;
-		}
-		else if(uContains(loopWords, id))
-		{
-			// PINK = FOUND IN LOOP SIGNATURE
-			color = Qt::magenta;
-		}
-		else if(_lastIds.contains(id))
-		{
-			// BLUE = FOUND IN LAST SIGNATURE
-			color = Qt::blue;
-		}
-		else if(id<=_lastId)
-		{
-			// RED = ALREADY EXISTS
-			color = Qt::red;
-		}
-		else if(refWords.count(id) > 1)
-		{
-			// YELLOW = NEW and multiple times
-			color = Qt::yellow;
-		}
-		else
-		{
-			// GREEN = NEW
-			color = Qt::green;
-		}
-		_ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
-	}
-	ULOGGER_DEBUG("source time = %f s", timer.ticks());
+	ULOGGER_DEBUG("source time (shown=%d) = %f s", _ui->imageView_source->isFeaturesShown()?1:0, timer.ticks());
 
 	timer.start();
 	ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
 	QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
-	if(loopWords.size())
+	_ui->imageView_loopClosure->clearFeatures();
+	if(_ui->imageView_loopClosure->isFeaturesShown())
 	{
-		_ui->imageView_loopClosure->clearFeatures();
-	}
-	for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
-	{
-		int id = iter->first;
-		QColor color;
-		if(id<0)
+		for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
 		{
-			// GRAY = NOT QUANTIZED
-			color = Qt::gray;
-		}
-		else if(uContains(refWords, id))
-		{
-			// PINK = FOUND IN LOOP SIGNATURE
-			color = Qt::magenta;
-			//To draw lines... get only unique correspondences
-			if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
+			int id = iter->first;
+			QColor color;
+			if(id<0)
 			{
-				const cv::KeyPoint & a = refWords.find(id)->second;
-				const cv::KeyPoint & b = iter->second;
-				uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
+				// GRAY = NOT QUANTIZED
+				color = Qt::gray;
+			}
+			else if(uContains(refWords, id))
+			{
+				// PINK = FOUND IN LOOP SIGNATURE
+				color = Qt::magenta;
+				//To draw lines... get only unique correspondences
+				if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
+				{
+					const cv::KeyPoint & a = refWords.find(id)->second;
+					const cv::KeyPoint & b = iter->second;
+					uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
+				}
+			}
+			else if(id<=_lastId)
+			{
+				// RED = ALREADY EXISTS
+				color = Qt::red;
+			}
+			else if(refWords.count(id) > 1)
+			{
+				// YELLOW = NEW and multiple times
+				color = Qt::yellow;
+			}
+			else
+			{
+				// GREEN = NEW
+				color = Qt::green;
+			}
+			_ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
+		}
+	}
+	else if(_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown())
+	{
+		for(std::multimap<int, cv::KeyPoint>::const_iterator iter = loopWords.begin(); iter != loopWords.end(); ++iter )
+		{
+			int id = iter->first;
+			if(id>=0 && uContains(refWords, id))
+			{
+				//To draw lines... get only unique correspondences
+				if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
+				{
+					const cv::KeyPoint & a = refWords.find(id)->second;
+					const cv::KeyPoint & b = iter->second;
+					uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(a.pt, b.pt));
+				}
 			}
 		}
-		else if(id<=_lastId)
-		{
-			// RED = ALREADY EXISTS
-			color = Qt::red;
-		}
-		else if(refWords.count(id) > 1)
-		{
-			// YELLOW = NEW and multiple times
-			color = Qt::yellow;
-		}
-		else
-		{
-			// GREEN = NEW
-			color = Qt::green;
-		}
-		_ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
 	}
+	ULOGGER_DEBUG("loop closure time (shown=%d) = %f s", _ui->imageView_loopClosure->isFeaturesShown()?1:0, timer.ticks());
 
-	ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
-
+	_lastIds.clear();
 	if(refWords.size()>0)
 	{
 		if((*refWords.rbegin()).first > _lastId)
@@ -5125,52 +5173,51 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 #endif
 	}
 
-	// Draw lines between corresponding features...
-	float scaleSource = _ui->imageView_source->viewScale();
-	float scaleLoop = _ui->imageView_loopClosure->viewScale();
-	UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
-	// Delta in actual window pixels
-	float sourceMarginX = (_ui->imageView_source->width()   - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
-	float sourceMarginY = (_ui->imageView_source->height()  - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
-	float loopMarginX   = (_ui->imageView_loopClosure->width()   - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
-	float loopMarginY   = (_ui->imageView_loopClosure->height()  - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
-
-	float deltaX = 0;
-	float deltaY = 0;
-
-	if(_preferencesDialog->isVerticalLayoutUsed())
+	_ui->imageView_source->clearLines();
+	_ui->imageView_loopClosure->clearLines();
+	if(_ui->imageView_source->isLinesShown() && _ui->imageView_loopClosure->isLinesShown())
 	{
-		deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
-	}
-	else
-	{
-		deltaX = _ui->imageView_source->width();
-	}
+		// Draw lines between corresponding features...
+		float scaleSource = _ui->imageView_source->viewScale();
+		float scaleLoop = _ui->imageView_loopClosure->viewScale();
+		UDEBUG("scale source=%f loop=%f", scaleSource, scaleLoop);
+		// Delta in actual window pixels
+		float sourceMarginX = (_ui->imageView_source->width()   - _ui->imageView_source->sceneRect().width()*scaleSource)/2.0f;
+		float sourceMarginY = (_ui->imageView_source->height()  - _ui->imageView_source->sceneRect().height()*scaleSource)/2.0f;
+		float loopMarginX   = (_ui->imageView_loopClosure->width()   - _ui->imageView_loopClosure->sceneRect().width()*scaleLoop)/2.0f;
+		float loopMarginY   = (_ui->imageView_loopClosure->height()  - _ui->imageView_loopClosure->sceneRect().height()*scaleLoop)/2.0f;
 
-	if(refWords.size() && loopWords.size())
-	{
-		_ui->imageView_source->clearLines();
-		_ui->imageView_loopClosure->clearLines();
-	}
+		float deltaX = 0;
+		float deltaY = 0;
 
-	for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
-		iter!=uniqueCorrespondences.end();
-		++iter)
-	{
+		if(_preferencesDialog->isVerticalLayoutUsed())
+		{
+			deltaY = _ui->label_matchId->height() + _ui->imageView_source->height();
+		}
+		else
+		{
+			deltaX = _ui->imageView_source->width();
+		}
 
-		_ui->imageView_source->addLine(
-				iter->first.x,
-				iter->first.y,
-				(iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
-				(iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
-				_ui->imageView_source->getDefaultMatchingLineColor());
+		for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
+			iter!=uniqueCorrespondences.end();
+			++iter)
+		{
 
-		_ui->imageView_loopClosure->addLine(
-				(iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
-				(iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
-				iter->second.x,
-				iter->second.y,
-				_ui->imageView_loopClosure->getDefaultMatchingLineColor());
+			_ui->imageView_source->addLine(
+					iter->first.x,
+					iter->first.y,
+					(iter->second.x*scaleLoop+loopMarginX+deltaX-sourceMarginX)/scaleSource,
+					(iter->second.y*scaleLoop+loopMarginY+deltaY-sourceMarginY)/scaleSource,
+					_ui->imageView_source->getDefaultMatchingLineColor());
+
+			_ui->imageView_loopClosure->addLine(
+					(iter->first.x*scaleSource+sourceMarginX-deltaX-loopMarginX)/scaleLoop,
+					(iter->first.y*scaleSource+sourceMarginY-deltaY-loopMarginY)/scaleLoop,
+					iter->second.x,
+					iter->second.y,
+					_ui->imageView_loopClosure->getDefaultMatchingLineColor());
+		}
 	}
 	_ui->imageView_source->update();
 	_ui->imageView_loopClosure->update();