The graph is also sent in appearance-only mode. Added posterior hypotheses color on the graph view. Some minor fixes.

corelib/src/DBDriverSqlite3.cpp

@@ -1019,7 +1019,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			// create the node
 			if(id)
 			{
-				ULOGGER_DEBUG("Creating %d (map=%d)", *iter, mapId);
+				ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
 				Signature * s = new Signature(
 						id,
 						mapId,

corelib/src/Memory.cpp

@@ -704,11 +704,6 @@ void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) cons
 	{
 		pose = s->getPose();
 		mapId = s->mapId();
-
-		if(pose.isNull())
-		{
-			UERROR("Pose of %d is null?!?", locationId);
-		}
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
@@ -1071,7 +1066,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 			likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
 		}
 
-		UDEBUG("compute likelihood... %f s", timer.ticks());
+		UDEBUG("compute likelihood (similarity)... %f s", timer.ticks());
 		return likelihood;
 	}
 	else
@@ -1147,7 +1142,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 			}
 		}
 
-		UDEBUG("compute likelihood %f s", timer.ticks());
+		UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks());
 		return likelihood;
 	}
 }
@@ -3558,11 +3553,8 @@ void Memory::getMetricConstraints(
 	{
 		Transform pose;
 		this->getPose(ids[i], pose, lookInDatabase);
-		if(!pose.isNull())
-		{
 		poses.insert(std::make_pair(ids[i], pose));
 	}
-	}
 
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
@@ -3571,7 +3563,7 @@ void Memory::getMetricConstraints(
 			std::map<int, Link> neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors
 			for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
 			{
-				if(!jter->second.transform().isNull() && uContains(poses, jter->first))
+				if(uContains(poses, jter->first))
 				{
 					bool edgeAlreadyAdded = false;
 					for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
@@ -3594,7 +3586,6 @@ void Memory::getMetricConstraints(
 			for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
 			{
 				if(jter->first < ids[i] &&
-					!jter->second.transform().isNull() &&
 					uContains(poses, jter->first))
 				{
 					links.insert(std::make_pair(ids[i],jter->second));

corelib/src/Rtabmap.cpp

@@ -1664,20 +1664,33 @@ bool Rtabmap::process(const SensorData & data)
 		statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
 
 		//Poses, place this after Transfer! (_optimizedPoses may change)
+		std::map<int, int> mapIds;
 		if(_rgbdSlamMode)
 		{
-			std::map<int, int> mapIds;
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
 				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
 			}
-
-			statistics_.setMapIds(mapIds);
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 		}
+		else if(_memory->getLastWorkingSignature())
+		{
+			// no optimization on appearance-only mode
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> constraints;
+			_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			}
+			statistics_.setPoses(poses);
+			statistics_.setConstraints(constraints);
+		}
+		statistics_.setMapIds(mapIds);
 	}
 
 	// Log info...
@@ -2128,12 +2141,18 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 			}
+		}
+		else
+		{
+			// no optimization on appearance-only mode
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
+			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+		}
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
 		}
-		}
 
 
 		// Get data
@@ -2187,24 +2206,19 @@ void Rtabmap::getGraph(
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 			}
+		}
+		else
+		{
+			// no optimization on appearance-only mode
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
+			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+		}
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
 		}
 	}
-		else
-		{
-			UWARN("Getting graph while not in RGB-D SLAM mode makes no sense... only map ids are set.");
-		}
-
-		std::set<int> ids = _memory->getWorkingMem(); // STM + WM
-		ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
-		if(global)
-		{
-			ids = _memory->getAllSignatureIds(); // STM + WM + LTM
-		}
-	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
 	{
 		UERROR("Last working signature is null!?");

corelib/src/RtabmapThread.cpp

@@ -174,6 +174,10 @@ void RtabmapThread::mainLoop()
 		break;
 	case kStateInit:
 		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
+		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		UASSERT(_dataBufferMaxSize >= 0);
+		UASSERT(_rate >= 0.0f);
 		_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
 		break;
 	case kStateChangingParameters:

corelib/src/util3d.cpp

@@ -2350,6 +2350,7 @@ void optimizeTOROGraph(
 		{
 			if(uContains(depthGraph, iter->first))
 			{
+				UASSERT(!iter->second.isNull());
 				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
 			}
 		}
@@ -2359,6 +2360,7 @@ void optimizeTOROGraph(
 		{
 			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
 			{
+				UASSERT(!iter->second.transform().isNull());
 				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
 			}
 		}
@@ -2433,6 +2435,7 @@ void optimizeTOROGraph(
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			float x,y,z, roll,pitch,yaw;
+			UASSERT(!iter->second.isNull());
 			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll,pitch,yaw);
 			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
 			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
@@ -2451,6 +2454,7 @@ void optimizeTOROGraph(
 			int id1 = iter->first;
 			int id2 = iter->second.to();
 			float x,y,z, roll,pitch,yaw;
+			UASSERT(!iter->second.transform().isNull());
 			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
 			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
 			AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
@@ -2939,6 +2943,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	{
 		if(uContains(occupancy, iter->first))
 		{
+			UASSERT(!iter->second.isNull());
 			const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
 
 			iter->second.getTranslationAndEulerAngles(x,y,z,toll,pitch,yaw);
@@ -3100,6 +3105,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 	{
 		if(uContains(scans, iter->first) && scans.at(iter->first)->size())
 		{
+			UASSERT(!iter->second.isNull());
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
 			pcl::PointXYZ min, max;
 			pcl::getMinMax3D(*cloud, min, max);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -120,6 +120,7 @@ public:
 	bool beepOnPause() const;
 	int getKeypointsOpacity() const;
 	int getOdomQualityWarnThr() const;
+	bool isPosteriorGraphView() const;
 
 	bool isGraphsShown() const;
 	bool isCloudMeshing() const;

guilib/src/GraphViewer.cpp

@@ -101,8 +101,10 @@ class LinkItem: public QGraphicsLineItem
 {
 public:
 	// in meter
-	LinkItem(const Transform & poseA, const Transform & poseB, bool loopClosure) :
+	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, bool loopClosure) :
 		QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()),
+		_from(from),
+		_to(to),
 		_poseA(poseA),
 		_poseB(poseB),
 		_loopClosure(loopClosure)
@@ -125,6 +127,8 @@ public:
 	}
 
 	bool isLoopClosure() const {return _loopClosure;}
+	int from() const {return _from;}
+	int to() const {return _to;}
 
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
@@ -138,6 +142,8 @@ protected:
 	}
 
 private:
+	int _from;
+	int _to;
 	Transform _poseA;
 	Transform _poseB;
 	bool _loopClosure;
@@ -195,22 +201,25 @@ GraphViewer::~GraphViewer()
 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints)
 {
+	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)
 	{
 		iter.value()->hide();
 		iter.value()->setColor(_nodeColor); // reset color
 	}
-	for(QMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end(); ++iter)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end(); ++iter)
 	{
 		iter.value()->hide();
 	}
-	for(QMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end(); ++iter)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end(); ++iter)
 	{
 		iter.value()->hide();
 	}
 
 	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(!iter->second.isNull())
 		{
 			QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
 			if(itemIter != _nodeItems.end())
@@ -230,33 +239,54 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				_nodeItems.insert(iter->first, item);
 			}
 		}
+	}
 
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jterA = poses.find(iter->first);
 		std::map<int, Transform>::const_iterator jterB = poses.find(iter->second.to());
-		if(jterA != poses.end() && jterB != poses.end())
+		if(jterA != poses.end() && jterB != poses.end() &&
+		   _nodeItems.contains(iter->first) && _nodeItems.contains(iter->second.to()))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
 			bool loopClosure = iter->second.type() != Link::kNeighbor;
 
+			bool added = false;
 			if(loopClosure && _loopLinkItems.contains(iter->first))
 			{
-				QMap<int, LinkItem*>::iterator itemIter = _loopLinkItems.find(iter->first);
+				QMultiMap<int, LinkItem*>::iterator itemIter = _loopLinkItems.find(iter->first);
+				while(itemIter.key() == iter->first && itemIter != _loopLinkItems.end())
+				{
+					if(itemIter.value()->to() == iter->second.to())
+					{
 						itemIter.value()->setPoses(poseA, poseB);
 						itemIter.value()->show();
+						added = true;
+						break;
+					}
+					++itemIter;
+				}
 			}
 			else if(!loopClosure && _neighborLinkItems.contains(iter->first))
 			{
-				QMap<int, LinkItem*>::iterator itemIter = _neighborLinkItems.find(iter->first);
+				QMultiMap<int, LinkItem*>::iterator itemIter = _neighborLinkItems.find(iter->first);
+				while(itemIter.key() == iter->first && itemIter != _neighborLinkItems.end())
+				{
+					if(itemIter.value()->to() == iter->second.to())
+					{
 						itemIter.value()->setPoses(poseA, poseB);
 						itemIter.value()->show();
+						added = true;
+						break;
 					}
-			else
+					++itemIter;
+				}
+			}
+			if(!added)
 			{
 				//create a link item
-				LinkItem * item = new LinkItem(poseA, poseB, loopClosure);
+				LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, loopClosure);
 				QPen p = item->pen();
 				p.setWidthF(_linkWidth);
 				item->setPen(p);
@@ -289,7 +319,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			++iter;
 		}
 	}
-	for(QMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end();)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end();)
 	{
 		if(!iter.value()->isVisible())
 		{
@@ -301,7 +331,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			++iter;
 		}
 	}
-	for(QMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end();)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end();)
 	{
 		if(!iter.value()->isVisible())
 		{
@@ -348,6 +378,35 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 	}
 }
 
+void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
+{
+	//find max
+	float max = 0.0f;
+	for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
+	{
+		if(iter->first > 0 && iter->second>max)
+		{
+			max = iter->second;
+		}
+	}
+	if(max > 0.0f)
+	{
+		for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+		{
+			std::map<int,float>::const_iterator jter = posterior.find(iter.key());
+			if(jter != posterior.end())
+			{
+				UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
+				iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
+			}
+			else
+			{
+				iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue
+			}
+		}
+	}
+}
+
 void GraphViewer::clearGraph()
 {
 	qDeleteAll(_nodeItems);
@@ -365,6 +424,14 @@ void GraphViewer::clearMap()
 	_gridMap->setPixmap(QPixmap());
 }
 
+void GraphViewer::clearPosterior()
+{
+	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+	{
+		iter.value()->setColor(Qt::blue); // blue
+	}
+}
+
 void GraphViewer::clearAll()
 {
 	clearMap();
@@ -523,7 +590,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	else if(r == aSetNodeSize)
 	{
 		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 1, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 100, 2, &ok);
 		if(ok)
 		{
 			_nodeRadius = value;
@@ -541,7 +608,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	else if(r == aSetLinkSize)
 	{
 		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 1, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 100, 2, &ok);
 		if(ok)
 		{
 			_linkWidth = value;

guilib/src/GraphViewer.h

@@ -51,8 +51,10 @@ public:
 	void updateGraph(const std::map<int, Transform> & poses,
 					 const std::multimap<int, Link> & constraints);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
+	void updatePosterior(const std::map<int, float> & posterior);
 	void clearGraph();
 	void clearMap();
+	void clearPosterior();
 	void clearAll();
 
 	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
@@ -68,8 +70,8 @@ private:
 	QColor _loopClosureColor;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
-	QMap<int, LinkItem*> _neighborLinkItems;
+	QMultiMap<int, LinkItem*> _neighborLinkItems;
-	QMap<int, LinkItem*> _loopLinkItems;
+	QMultiMap<int, LinkItem*> _loopLinkItems;
 	float _nodeRadius;
 	float _linkWidth;
 	QGraphicsPixmapItem * _gridMap;

guilib/src/MainWindow.cpp

@@ -519,26 +519,6 @@ void MainWindow::handleEvent(UEvent* anEvent)
 				this->pauseDetection();
 			}
 		}
-		UDEBUG("stat.rawLikelihood().size()=%d", stats.rawLikelihood().size());
-		// Performance issue: don't process the pdf and likelihood if the last event is
-		// not yet completely processed, to avoid an unresponsive GUI when events accumulate.
-		if(_processingStatistics || !_ui->dockWidget_posterior->isVisible())
-		{
-			stats.setPosterior(std::map<int, float>());
-		}
-		if(_processingStatistics || !_ui->dockWidget_likelihood->isVisible())
-		{
-			stats.setLikelihood(std::map<int, float>());
-		}
-		if(_processingStatistics || !_ui->dockWidget_rawlikelihood->isVisible())
-		{
-			stats.setRawLikelihood(std::map<int, float>());
-		}
-		if(_processingStatistics || (!_ui->dockWidget_posterior->isVisible() && !_ui->dockWidget_likelihood->isVisible()))
-		{
-			stats.setWeights(std::map<int,int>());
-		}
-
 		emit statsReceived(stats);
 	}
 	else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
@@ -1058,6 +1038,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				UDEBUG("time= %d ms", time.restart());
 			}
 		}
+
+		// update posterior on the graph view
+		if(_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible() && stat.posterior().size())
+		{
+			_ui->graphicsView_graphView->updatePosterior(stat.posterior());
+		}
+
 		UDEBUG("");
 	}
 	else if(!stat.extended() && stat.loopClosureId()>0)
@@ -1082,6 +1069,8 @@ void MainWindow::updateMapCloud(
 		const std::map<int, int> & mapIdsIn,
 		bool verboseProgress)
 {
+	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d currentPose=%s",
+			(int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), currentPose.prettyPrint().c_str());
 	if(posesIn.size())
 	{
 		_currentPosesMap = posesIn;
@@ -1243,10 +1232,6 @@ void MainWindow::updateMapCloud(
 				QApplication::processEvents();
 			}
 		}
-		else
-		{
-			UERROR("transform is null!?");
-		}
 
 		++i;
 	}
@@ -1299,7 +1284,7 @@ void MainWindow::updateMapCloud(
 	}
 
 	// Update occupancy grid map in 3D map view and graph view
-	if(_ui->graphicsView_graphView->isVisible() && constraints.size())
+	if(_ui->graphicsView_graphView->isVisible())
 	{
 		_ui->graphicsView_graphView->updateGraph(posesIn, constraints);
 	}
@@ -1377,6 +1362,7 @@ void MainWindow::updateMapCloud(
 
 void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
 {
+	UASSERT(!pose.isNull());
 	std::string cloudName = uFormat("cloud%d", nodeId);
 	if(_ui->widget_cloudViewer->getAddedClouds().contains(cloudName))
 	{
@@ -1602,19 +1588,46 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
 		   !_emptyNewDatabase)
 		{
 			// Temp database used, automatically backup with unique name (timestamp)
-			QString newName = _preferencesDialog->getWorkingDirectory()+QDir::separator()+(QString("rtabmap_%1.db").arg(QDateTime::currentDateTime().toString("yyMMdd-hhmmsszzz")));
+			QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmsszzz");
-			if(QFile::rename(_openedDatabasePath, newName))
+			bool ok = false;
+			newName = QInputDialog::getText(this, tr("Saving database..."), tr("Database name:"), QLineEdit::Normal, newName, &ok);
+			while(ok)
 			{
-				std::string msg = uFormat("Database saved to \"%s\".", newName.toStdString().c_str());
+				QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
+				if(QFile::exists(newPath))
+				{
+					QMessageBox::StandardButton b = QMessageBox::question(this,
+							tr("Saving database..."),
+							tr("Database \"%1\" already exists, do you want to overwrite?").arg(newName+".db"),
+							QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
+					if(b == QMessageBox::Yes && QFile::remove(newPath))
+					{
+						UINFO("Deleted database \"%s\".", newPath.toStdString().c_str());
+					}
+					else
+					{
+						if(b == QMessageBox::Yes)
+						{
+							UERROR("Failed to erase database \"%s\"! Asking for new name...", newPath.toStdString().c_str());
+						}
+						newName = QInputDialog::getText(this, tr("Saving database..."), tr("Database name:"), QLineEdit::Normal, newName, &ok);
+						continue;
+					}
+				}
+				if(QFile::rename(_openedDatabasePath, newPath))
+				{
+					std::string msg = uFormat("Database saved to \"%s\".", newPath.toStdString().c_str());
 					UINFO(msg.c_str());
 					QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
 				}
 				else
 				{
-				std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".", _openedDatabasePath.toStdString().c_str(), newName.toStdString().c_str());
+					std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".", _openedDatabasePath.toStdString().c_str(), (newName+".db").toStdString().c_str());
 					UERROR(msg.c_str());
 					QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
 				}
+				break;
+			}
 		}
 		_openedDatabasePath.clear();
 		bool applicationClosing = _state == kApplicationClosing;
@@ -1751,6 +1764,10 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 	{
 		UDEBUG("General settings changed...");
 		setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
+		if(!_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible())
+		{
+			_ui->graphicsView_graphView->clearPosterior();
+		}
 	}
 
 	if(flags & PreferencesDialog::kPanelCloudRendering)
@@ -1758,7 +1775,11 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		UDEBUG("Cloud rendering settings changed...");
 		if(_currentPosesMap.size())
 		{
-			this->updateMapCloud(_currentPosesMap, Transform(), _currentLinksMap, _currentMapIds);
+			this->updateMapCloud(
+					std::map<int, Transform>(_currentPosesMap),
+					Transform(),
+					std::multimap<int, Link>(_currentLinksMap),
+					std::map<int, int>(_currentMapIds));
 		}
 	}
 
@@ -3105,7 +3126,7 @@ void MainWindow::postProcessing()
 	_initProgressDialog->incrementStep();
 
 	_initProgressDialog->appendText(tr("Updating map..."));
-	this->updateMapCloud(optimizedPoses, Transform(), _currentLinksMap, _currentMapIds, false);
+	this->updateMapCloud(optimizedPoses, Transform(), std::multimap<int, Link>(_currentLinksMap), std::map<int, int>(_currentMapIds), false);
 	_initProgressDialog->appendText(tr("Updating map... done!"));
 
 	_initProgressDialog->setValue(_initProgressDialog->maximumSteps());

guilib/src/PreferencesDialog.cpp

@@ -153,6 +153,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_beep, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->horizontalSlider_keypointsOpacity, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
 	// Cloud rendering panel
 	_3dRenderingShowClouds.resize(2);
@@ -554,7 +555,6 @@ void PreferencesDialog::init()
 
 	this->loadWindowGeometry("PreferencesDialog", this);
 
-	_obsoletePanels = kPanelAll;
 	_initialized = true;
 }
 
@@ -819,6 +819,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
 		_ui->horizontalSlider_keypointsOpacity->setSliderPosition(20);
 		_ui->spinBox_odomQualityWarnThr->setValue(50);
+		_ui->checkBox_posteriorGraphView->setChecked(true);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_cloudRendering1->objectName())
 	{
@@ -1059,6 +1060,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_beep->setChecked(settings.value("beep", _ui->checkBox_beep->isChecked()).toBool());
 	_ui->horizontalSlider_keypointsOpacity->setValue(settings.value("keypointsOpacity", _ui->horizontalSlider_keypointsOpacity->value()).toInt());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
+	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -1269,6 +1271,8 @@ void PreferencesDialog::writeSettings(const QString & filePath)
 	writeCameraSettings(filePath);
 	writeCoreSettings(filePath);
 
+	UDEBUG("_obsoletePanels=%d parameters=%d", (int)_obsoletePanels, (int)_parameters.size());
+
 	if(_parameters.size())
 	{
 		emit settingsChanged(_parameters);
@@ -1307,6 +1311,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 	settings.setValue("beep", _ui->checkBox_beep->isChecked());
 	settings.setValue("keypointsOpacity", _ui->horizontalSlider_keypointsOpacity->value());
 	settings.setValue("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value());
+	settings.setValue("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -2651,6 +2656,10 @@ int PreferencesDialog::getOdomQualityWarnThr() const
 {
 	return _ui->spinBox_odomQualityWarnThr->value();
 }
+bool PreferencesDialog::isPosteriorGraphView() const
+{
+	return _ui->checkBox_posteriorGraphView->isChecked();
+}
 
 bool PreferencesDialog::isCloudsShown(int index) const
 {

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>744</width>
+            <width>732</width>
-            <height>1056</height>
+            <height>1302</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>23</number>
+                <number>0</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -97,10 +97,7 @@
                    </property>
                    <layout class="QVBoxLayout" name="verticalLayout_28">
                     <item>
-                     <layout class="QFormLayout" name="formLayout_14">
+                     <layout class="QGridLayout" name="gridLayout_39" columnstretch="0,1">
-                      <property name="fieldGrowthPolicy">
-                       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                      </property>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_imagesKept">
                         <property name="text">
@@ -145,24 +142,7 @@
                       <property name="title">
                        <string>Loop closure detection view</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_12">
+                      <layout class="QGridLayout" name="gridLayout_31" columnstretch="0,1">
-                       <item row="1" column="0">
-                        <widget class="QCheckBox" name="checkBox_imageFlipped">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_80">
-                         <property name="text">
-                          <string>Flip image (like a mirror).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>false</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="0">
                         <widget class="QCheckBox" name="checkBox_imageRejectedShown">
                          <property name="text">
@@ -173,10 +153,10 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="3" column="1">
-                        <widget class="QLabel" name="label_73">
+                        <widget class="QLabel" name="label_105">
                          <property name="text">
-                          <string>Show image when an hypothesis is rejected.</string>
+                          <string>Show the image of the highest hypothesis.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -193,17 +173,17 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_105">
+                        <widget class="QLabel" name="label_80">
                          <property name="text">
-                          <string>Show the image of the highest hypothesis.</string>
+                          <string>Flip image (like a mirror).</string>
                          </property>
                          <property name="wordWrap">
-                          <bool>true</bool>
+                          <bool>false</bool>
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
+                       <item row="5" column="0">
                         <widget class="QSlider" name="horizontalSlider_keypointsOpacity">
                          <property name="minimum">
                           <number>0</number>
@@ -219,10 +199,10 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="1">
+                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_102">
+                        <widget class="QLabel" name="label_73">
                          <property name="text">
-                          <string>Keypoints opacity.</string>
+                          <string>Show image when an hypothesis is rejected.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -249,6 +229,43 @@
                          </property>
                         </widget>
                        </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="checkBox_imageFlipped">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_102">
+                         <property name="text">
+                          <string>Keypoints opacity.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QLabel" name="label_222">
+                         <property name="text">
+                          <string>Show loop closure hypotheses on the graph view. Nodes are colorized from blue to red depending on the highest hypothesis (which is red). Posterior hypotheses should be published. </string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="0">
+                        <widget class="QCheckBox" name="checkBox_posteriorGraphView">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -257,7 +274,7 @@
                       <property name="title">
                        <string>3D Map view</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_23">
+                      <layout class="QGridLayout" name="gridLayout_38" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="spinBox_odomQualityWarnThr">
                          <property name="maximum">
@@ -1121,10 +1138,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                    <property name="title">
                     <string>Logging</string>
                    </property>
-                   <layout class="QFormLayout" name="formLayout_11">
+                   <layout class="QGridLayout" name="gridLayout_40" columnstretch="0,1">
-                    <property name="fieldGrowthPolicy">
-                     <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                    </property>
                     <item row="0" column="0">
                      <widget class="QComboBox" name="comboBox_loggerLevel">
                       <property name="currentIndex">
@@ -2282,10 +2296,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    </property>
                    <layout class="QVBoxLayout" name="verticalLayout_15">
                     <item>
-                     <layout class="QFormLayout" name="formLayout_17">
+                     <layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
-                      <property name="fieldGrowthPolicy">
-                       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                      </property>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_SLAM_mode">
                         <property name="text">
@@ -2330,16 +2341,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_84">
-                        <property name="text">
-                         <string>Data buffer size (0 means inf).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="2" column="0">
                        <widget class="QSpinBox" name="general_spinBox_imagesBufferSize">
                         <property name="maximum">
@@ -2350,6 +2351,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_84">
+                        <property name="text">
+                         <string>Data buffer size (0 means inf).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="3" column="0">
                        <widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
                         <property name="text">
@@ -2377,10 +2388,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="title">
                        <string>Thresholds</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_18">
+                      <layout class="QGridLayout" name="gridLayout_44" columnstretch="0,1">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="general_doubleSpinBox_timeThr">
                          <property name="suffix">
@@ -2487,10 +2495,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="checkable">
                        <bool>true</bool>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_15">
+                      <layout class="QGridLayout" name="gridLayout_45" columnstretch="0,1">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
                        <item row="0" column="0">
                         <widget class="QCheckBox" name="general_checkBox_publishRawData">
                          <property name="text">
@@ -2553,10 +2558,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="checkable">
                        <bool>true</bool>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_16">
+                      <layout class="QGridLayout" name="gridLayout_46" columnstretch="0,1">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
                        <item row="0" column="0">
                         <widget class="QCheckBox" name="general_checkBox_statisticLogsBufferedInRAM">
                          <property name="text">
@@ -2808,10 +2810,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    </property>
                    <layout class="QVBoxLayout" name="verticalLayout_10">
                     <item>
-                     <layout class="QFormLayout" name="formLayout_10">
+                     <layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
-                      <property name="fieldGrowthPolicy">
-                       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                      </property>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -2927,16 +2926,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_retrieved_4">
-                        <property name="text">
-                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="5" column="0">
                        <widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
                         <property name="text">
@@ -2947,6 +2936,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_retrieved_4">
+                        <property name="text">
+                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -3235,10 +3234,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                    </property>
                    <layout class="QVBoxLayout" name="verticalLayout_12">
                     <item>
-                     <layout class="QFormLayout" name="formLayout_7">
+                     <layout class="QGridLayout" name="gridLayout_41" columnstretch="0,1">
-                      <property name="fieldGrowthPolicy">
-                       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                      </property>
                       <item row="0" column="0">
                        <widget class="QComboBox" name="comboBox_detector_strategy">
                         <item>
@@ -3395,14 +3391,14 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="0">
+                      <item row="4" column="0">
                        <widget class="QLineEdit" name="lineEdit_kp_roi">
                         <property name="readOnly">
                          <bool>true</bool>
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="4" column="1">
                        <widget class="QLabel" name="label_101">
                         <property name="text">
                          <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
@@ -3412,7 +3408,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
+                      <item row="5" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
                         <property name="suffix">
                          <string> %</string>
@@ -3422,7 +3418,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="1">
+                      <item row="5" column="1">
                        <widget class="QLabel" name="label_97">
                         <property name="text">
                          <string>Left ROI ratio (0 = no change).</string>
@@ -3432,7 +3428,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="6" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
                         <property name="suffix">
                          <string> %</string>
@@ -3442,7 +3438,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="6" column="1">
                        <widget class="QLabel" name="label_98">
                         <property name="text">
                          <string>Right ROI ratio (0 = no change).</string>
@@ -3452,7 +3448,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="0">
+                      <item row="7" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi2">
                         <property name="suffix">
                          <string> %</string>
@@ -3462,7 +3458,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
+                      <item row="7" column="1">
                        <widget class="QLabel" name="label_99">
                         <property name="text">
                          <string>Top ROI ratio (0 = no change).</string>
@@ -3472,7 +3468,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="8" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi3">
                         <property name="suffix">
                          <string> %</string>
@@ -3482,7 +3478,7 @@ generate the number of words requested.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_100">
                         <property name="text">
                          <string>Bottom ROI ratio (0 = no change).</string>