DbViewer: In graph view, nodes in latest working memory state are highlighted in red

corelib/include/rtabmap/core/DBDriver.h

@@ -168,6 +168,7 @@ public:
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
+	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
@@ -265,6 +266,7 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -136,6 +136,7 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;

corelib/src/DBDriver.cpp

@@ -816,6 +816,13 @@ void DBDriver::getWeight(int signatureId, int & weight) const
 	}
 }
 
+void DBDriver::getLastNodeIds(std::set<int> & ids) const
+{
+	_dbSafeAccessMutex.lock();
+	this->getLastNodeIdsQuery(ids);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
 {
 	// look in the trash

corelib/src/DBDriverSqlite3.cpp

@@ -2324,6 +2324,49 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 	return found;
 }
 
+void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
+{
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		if(uStrNumCmp(_version, "0.11.11") >= 0)
+		{
+			query = "SELECT n.id "
+					 "FROM Node AS n "
+					 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Info) "
+					 "ORDER BY n.id;";
+		}
+		else
+		{
+			query = "SELECT n.id "
+					 "FROM Node AS n "
+					 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
+					 "ORDER BY n.id;";
+		}
+
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			ids.insert(ids.end(), sqlite3_column_int(ppStmt, 0)); // Signature Id
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
+	}
+}
 
 void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const
 {

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -193,6 +193,7 @@ private:
 	CloudViewer * stereoViewer_;
 	CloudViewer * occupancyGridViewer_;
 	QList<int> ids_;
+	std::set<int> lastWmIds_;
 	std::map<int, int> mapIds_;
 	std::map<int, int> weights_;
 	std::map<int, std::vector<int> > wmStates_;

guilib/include/rtabmap/gui/GraphViewer.h

@@ -61,14 +61,15 @@ public:
 
 	void updateGraph(const std::map<int, Transform> & poses,
 					 const std::multimap<int, Link> & constraints,
-					 const std::map<int, int> & mapIds);
+					 const std::map<int, int> & mapIds,
+					 const std::map<int, int> & weights = std::map<int, int>());
 	void updateGTGraph(const std::map<int, Transform> & poses);
 	void updateGPSGraph(
 			const std::map<int, Transform> & gpsMapPoses,
 			const std::map<int, GPS> & gpsValues);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
-	void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f);
+	void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());

guilib/src/DatabaseViewer.cpp

@@ -945,6 +945,7 @@ bool DatabaseViewer::closeDatabase()
 		groundTruthPoses_.clear();
 		gpsPoses_.clear();
 		gpsValues_.clear();
+		lastWmIds_.clear();
 		mapIds_.clear();
 		weights_.clear();
 		wmStates_.clear();
@@ -1493,6 +1494,8 @@ void DatabaseViewer::updateIds()
 	std::set<int> ids;
 	dbDriver_->getAllNodeIds(ids);
 	ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
+	lastWmIds_.clear();
+	dbDriver_->getLastNodeIds(lastWmIds_);
 	idToIndex_.clear();
 	mapIds_.clear();
 	weights_.clear();
@@ -5342,7 +5345,27 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
 		ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
-		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
+		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
+		if(!ui_->checkBox_wmState->isChecked())
+		{
+			bool allNodesAreInWM = true;
+			std::map<int, float> colors;
+			for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
+			{
+				if(lastWmIds_.find(iter->first) != lastWmIds_.end())
+				{
+					colors.insert(std::make_pair(iter->first, 1));
+				}
+				else
+				{
+					allNodesAreInWM = false;
+				}
+			}
+			if(!allNodesAreInWM)
+			{
+				ui_->graphViewer->updatePosterior(colors, 1, 1);
+			}
+		}
 		ui_->graphViewer->clearMap();
 		occupancyGridViewer_->clear();
 		if(graph.size() && localMaps.size() &&

guilib/src/GraphViewer.cpp

@@ -60,10 +60,11 @@ class NodeItem: public QGraphicsEllipseItem
 {
 public:
 	// in meter
-	NodeItem(int id, int mapId, const Transform & pose, float radius) :
+	NodeItem(int id, int mapId, const Transform & pose, float radius, int weight=-1) :
 		QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
 		_id(id),
 		_mapId(mapId),
+		_weight(weight),
 		_pose(pose),
 		_line(0)
 	{
@@ -106,7 +107,14 @@ public:
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
 	{
-		this->setToolTip(QString("%1 [%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
+		if(_weight>=0)
+		{
+			this->setToolTip(QString("%1 [map=%2, w=%3] %4").arg(_id).arg(_mapId).arg(_weight).arg(_pose.prettyPrint().c_str()));
+		}
+		else
+		{
+			this->setToolTip(QString("%1 [map=%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
+		}
 		this->setScale(2);
 		QGraphicsEllipseItem::hoverEnterEvent(event);
 	}
@@ -120,6 +128,7 @@ protected:
 private:
 	int _id;
 	int _mapId;
+	int _weight;
 	Transform _pose;
 	QGraphicsLineItem * _line;
 };
@@ -343,7 +352,8 @@ void GraphViewer::setWorldMapRotation(const float & theta)
 
 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints,
-				 const std::map<int, int> & mapIds)
+				 const std::map<int, int> & mapIds,
+				 const std::map<int, int> & weights)
 {
 	UTimer timer;
 	bool wasVisible = _graphRoot->isVisible();
@@ -356,6 +366,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	{
 		iter.value()->hide();
 		iter.value()->setColor(iter.key()<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor); // reset color
+		iter.value()->setZValue(iter.key()<0?21:20);
 	}
 	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
 	{
@@ -376,7 +387,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			{
 				// create node item
 				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius);
+				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);
 				this->scene()->addItem(item);
 				item->setZValue(iter->first<0?21:20);
 				item->setColor(iter->first<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor);
@@ -856,7 +867,7 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 	}
 }
 
-void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max)
+void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max, int zValueOffset)
 {
 	//find max
 	if(max <= 0.0f)
@@ -878,8 +889,9 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float
 			{
 				float v = jter->second>max?max:jter->second;
 				iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
+				iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
 			}
-			else
+			else if(iter.key() > 0)
 			{
 				iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue
 			}