Fixed labels not correctly shown. Fixed planner not sending next goals when memory management is disabled.

corelib/include/rtabmap/core/Memory.h

@@ -160,7 +160,7 @@ public:
 	const Signature * getLastWorkingSignature() const;
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);
-	std::map<int, std::string> getAllLabels() const;
+	const std::map<int, std::string> & getAllLabels() const {return _labels;}
 	bool allNodesInWM() const {return _allNodesInWM;}
 
 	/**
@@ -328,6 +328,7 @@ private:
 	std::set<int> _stMem; // id
 	std::map<int, double> _workingMem; // id,age
 	std::map<int, Transform> _groundTruths;
+	std::map<int, std::string> _labels;
 
 	//Keypoint stuff
 	VWDictionary * _vwd;

corelib/include/rtabmap/core/Statistics.h

@@ -192,6 +192,7 @@ public:
 	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
 	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
 	void setLocalizationCovariance(const cv::Mat & covariance) {_localizationCovariance = covariance;}
+	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -218,6 +219,7 @@ public:
 	const Transform & mapCorrection() const {return _mapCorrection;}
 	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
 	const cv::Mat & localizationCovariance() const {return _localizationCovariance;}
+	const std::map<int, std::string> & labels() const {return _labels;}
 	const std::map<int, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -248,6 +250,7 @@ private:
 	Transform _loopClosureTransform;
 	cv::Mat _localizationCovariance;
 
+	std::map<int, std::string> _labels;
 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;
 	std::map<int, float> _likelihood;

corelib/src/Memory.cpp

@@ -249,6 +249,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			}
 		}
 
+		// Get labels
+		UDEBUG("Get labels");
+		_dbDriver->getAllLabels(_labels);
+
 		UDEBUG("Check if all nodes are in Working Memory");
 		for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end() && _allNodesInWM; ++iter)
 		{
@@ -923,6 +927,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 					{
 						UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
 						signature->setLabel(tag);
+						_labels.insert(std::make_pair(signature->id(), tag));
 					}
 				}
 			}
@@ -935,6 +940,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 			{
 				UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
 				signature->setLabel(tag);
+				_labels.insert(std::make_pair(signature->id(), tag));
 			}
 		}
 
@@ -1593,6 +1599,7 @@ void Memory::clear()
 	_rectStereoCameraModel = StereoCameraModel();
 	_odomMaxInf.clear();
 	_groundTruths.clear();
+	_labels.clear();
 	_allNodesInWM = true;
 
 	if(_dbDriver)
@@ -2286,6 +2293,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 		Signature * s  = this->_getSignature(id);
 		if(s)
 		{
+			uInsert(_labels, std::make_pair(s->id(), label));
 			s->setLabel(label);
 			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
 			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
@@ -2299,6 +2307,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 			_dbDriver->loadSignatures(ids,signatures);
 			if(signatures.size())
 			{
+				uInsert(_labels, std::make_pair(signatures.front()->id(), label));
 				signatures.front()->setLabel(label);
 				UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 				_dbDriver->asyncSave(signatures.front()); // move it again to trash
@@ -2317,23 +2326,6 @@ bool Memory::labelSignature(int id, const std::string & label)
 	return false;
 }
 
-std::map<int, std::string> Memory::getAllLabels() const
-{
-	std::map<int, std::string> labels;
-	for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
-	{
-		if(!iter->second->getLabel().empty())
-		{
-			labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
-		}
-	}
-	if(_dbDriver)
-	{
-		_dbDriver->getAllLabels(labels);
-	}
-	return labels;
-}
-
 bool Memory::setUserData(int id, const cv::Mat & data)
 {
 	Signature * s  = this->_getSignature(id);

corelib/src/Rtabmap.cpp

@@ -1807,54 +1807,57 @@ bool Rtabmap::process(
 					timeGetNeighborsSpaceDb);
 
 		}
+	}
 
-		//============================================================
+	//============================================================
-		// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
+	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
-		//============================================================
+	//============================================================
-		std::list<int> retrievalLocalIds;
+	std::list<int> retrievalLocalIds;
-		if(_rgbdSlamMode)
+	if(_rgbdSlamMode)
+	{
+		// Priority on locations on the planned path
+		if(_path.size())
 		{
-			// Priority on locations on the planned path
+			updateGoalIndex();
-			if(_path.size())
+
+			float distanceSoFar = 0.0f;
+			// immunize all nodes after current node and
+			// retrieve nodes after current node in the maximum radius from the current node
+			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
-				updateGoalIndex();
+				if(_localRadius > 0.0f && i != _pathCurrentIndex)
-
-				float distanceSoFar = 0.0f;
-				// immunize all nodes after current node and
-				// retrieve nodes after current node in the maximum radius from the current node
-				for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 				{
-					if(_localRadius > 0.0f && i != _pathCurrentIndex)
+					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-					{
+				}
-						distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-					}
 
-					if(distanceSoFar <= _localRadius)
+				if(distanceSoFar <= _localRadius)
+				{
+					if(_memory->getSignature(_path[i].first) != 0)
 					{
-						if(_memory->getSignature(_path[i].first) != 0)
+						if(immunizedLocations.insert(_path[i].first).second)
 						{
-							if(immunizedLocations.insert(_path[i].first).second)
+							++immunizedLocally;
-							{
-								++immunizedLocally;
-							}
-							UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
-						}
-						else if(retrievalLocalIds.size() < _maxLocalRetrieved)
-						{
-							UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-							retrievalLocalIds.push_back(_path[i].first);
-							// retrieved locations are automatically immunized
 						}
+						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 					}
-					else
+					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
 					{
-						UDEBUG("Stop on node %d (dist=%fm > %fm)",
+						UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-								_path[i].first, distanceSoFar, _localRadius);
+						retrievalLocalIds.push_back(_path[i].first);
-						break;
+						// retrieved locations are automatically immunized
 					}
 				}
+				else
+				{
+					UDEBUG("Stop on node %d (dist=%fm > %fm)",
+							_path[i].first, distanceSoFar, _localRadius);
+					break;
+				}
 			}
+		}
 
+		if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
+		{
 			// immunize the path from the nearest local location to the current location
 			if(immunizedLocally < maxLocalLocationsImmunized &&
 				_memory->isIncremental()) // Can only work in mapping mode
@@ -2739,6 +2742,8 @@ bool Rtabmap::process(
 				}
 			}
 
+			statistics_.setLabels(_memory->getAllLabels());
+
 			// Path
 			if(_path.size())
 			{

guilib/src/MainWindow.cpp

@@ -1881,9 +1881,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				groundTruth.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getGroundTruthPose()));
 			}
-			if(!stat.getLastSignatureData().getLabel().empty())
+			for(std::map<int, std::string>::const_iterator iter=stat.labels().begin(); iter!=stat.labels().end(); ++iter)
 			{
-				labels.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getLabel()));
+				uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified
 			}
 			if(_ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
 				stat.getLastSignatureData().sensorData().gps().stamp()!=0.0 &&