Refactored graph::getPosesInRadius -> graph::findNearestPoses. Labels can be removed: added Remove label option in MainWindow. In localization mode, label set with id=0 is set to nearest node of current pose.

corelib/src/Graph.cpp

@@ -2080,12 +2080,12 @@ std::list<std::pair<int, Transform> > computePath(
 }
 
 int findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance)
 {
 	int id = 0;
-	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, 0, 0, 1);
 	if(!nearestNodes.empty())
 	{
 		id = nearestNodes.begin()->first;
@@ -2097,70 +2097,44 @@ int findNearestNode(
 	return id;
 }
 
+// return <id, sqrd distance>, excluding query
 std::map<int, float> findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
 		int k)
 {
-	std::map<int, float> nearestIds;
-	if(nodes.size() && !targetPose.isNull())
-	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(nodes.size());
-		std::vector<int> ids(nodes.size());
-		int oi = 0;
-		for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			ids[oi++] = iter->first;
-		}
+	UASSERT(uContains(poses, nodeId));
 
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> dist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->nearestKSearch(pt, k, ind, dist);
-
-		for(unsigned int i=0; i<ind.size(); ++i)
-		{
-			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
-		}
-	}
-	return nearestIds;
-}
-
-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
+	std::map<int, Transform> nodesMinusTarget = poses;
+	Transform targetPose = poses.at(nodeId);
 	nodesMinusTarget.erase(nodeId);
-	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+	return findNearestNodes(targetPose, nodesMinusTarget, radius, angle, k);
 }
 
-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
+// return <id, sqrd distance>
+std::map<int, float> findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius)
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
 {
+	UASSERT(radius>=0.0f);
+	UASSERT(k>=0);
+	UASSERT(radius > 0.0f || k>0);
 	std::map<int, float> foundNodes;
-	if(nodes.empty())
+	if(poses.empty())
 	{
 		return foundNodes;
 	}
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
-	std::vector<int> ids(nodes.size());
+	cloud->resize(poses.size());
+	std::vector<int> ids(poses.size());
 	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
@@ -2177,89 +2151,33 @@ std::map<int, float> getNodesInRadius(
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
-		for(unsigned int i=0; i<ind.size(); ++i)
+		if(radius>0.0f)
 		{
-			if(ind[i] >=0)
-			{
-				foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
-			}
+			kdTree->radiusSearch(pt, radius, ind, sqrdDist, k);
+		}
+		else
+		{
+			kdTree->nearestKSearch(pt, k, ind, sqrdDist);
 		}
-	}
-	UDEBUG("found nodes=%d", (int)foundNodes.size());
-	return foundNodes;
-}
-
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
-	nodesMinusTarget.erase(nodeId);
-	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
-}
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	std::map<int, Transform> foundNodes;
-	if(nodes.empty())
-	{
-		return foundNodes;
-	}
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
-	std::vector<int> ids(nodes.size());
-	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-		ids[oi] = iter->first;
-		++oi;
-	}
-	cloud->resize(oi);
-	ids.resize(oi);
-
-	if(cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
-
 		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
 			{
 				if(angle > 0.0f)
 				{
-					const Transform & checkT = nodes.at(ids[ind[i]]);
+					const Transform & checkT = poses.at(ids[ind[i]]);
 					// same orientation?
 					Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 					if(a <= angle)
 					{
-						foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+						foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 					}
 				}
 				else
 				{
-					foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+					foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 				}
 			}
 		}
@@ -2268,6 +2186,62 @@ std::map<int, Transform> getPosesInRadius(
 	return foundNodes;
 }
 
+// return <id, Transform>, excluding query
+std::map<int, Transform> findNearestPoses(
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	UASSERT(uContains(poses, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = poses;
+	Transform targetPose = poses.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return findNearestPoses(targetPose, nodesMinusTarget, radius, angle, k);
+}
+// return <id, Transform>
+std::map<int, Transform> findNearestPoses(
+		const Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, radius, angle, k);
+	std::map<int, Transform> foundPoses;
+	for(std::map<int, float>::iterator iter=nearestNodes.begin(); iter!=nearestNodes.end(); ++iter)
+	{
+		foundPoses.insert(*poses.find(iter->first));
+	}
+	UDEBUG("found nodes=%d", (int)foundPoses.size());
+	return foundPoses;
+}
+
+// deprecated stuff
+std::map<int, float> findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k)
+{
+	return findNearestNodes(targetPose, nodes, 0, 0, k);
+}
+std::map<int, float> getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(nodeId, nodes, radius);
+}
+std::map<int, float> getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(targetPose, nodes, radius);
+}
+std::map<int, Transform> getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(nodeId, nodes, radius, angle);
+}
+std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(targetPose, nodes, radius, angle);
+}
+
+
 float computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex,

corelib/src/Memory.cpp

@@ -2523,6 +2523,14 @@ int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase
 		if(id == 0 && _dbDriver && lookInDatabase)
 		{
 			_dbDriver->getNodeIdByLabel(label, id);
+			if(_signatures.find(id) != _signatures.end())
+			{
+				// The signature is already in WM, but label was not
+				// found above. It means the label has been cleared in
+				// current session (not yet saved to database), so return
+				// not found.
+				id = 0;
+			}
 		}
 	}
 	return id;
@@ -2532,15 +2540,35 @@ bool Memory::labelSignature(int id, const std::string & label)
 {
 	// verify that this label is not used
 	int idFound=getSignatureIdByLabel(label);
+	if(idFound == 0 && label.empty() && _labels.find(id)==_labels.end())
+	{
+		UWARN("Trying to remove label from node %d but it has already no label", id);
+		return false;
+	}
 	if(idFound == 0 || idFound == id)
 	{
 		Signature * s  = this->_getSignature(id);
 		if(s)
 		{
-			uInsert(_labels, std::make_pair(s->id(), label));
+			if(label.empty())
+			{
+				UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+				_labels.erase(id);
+			}
+			else
+			{
+				if(_labels.find(id)!=_labels.end())
+				{
+					UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+				}
+				else
+				{
+					UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+				}
+				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);
 			return true;
 		}
 		else if(_dbDriver)
@@ -2551,9 +2579,25 @@ 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));
+				if(label.empty())
+				{
+					UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+					_labels.erase(id);
+				}
+				else
+				{
+					if(_labels.find(id)!=_labels.end())
+					{
+						UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+					}
+					else
+					{
+						UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+					}
+					uInsert(_labels, std::make_pair(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
 				return true;
 			}
@@ -2565,7 +2609,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 	}
 	else if(idFound)
 	{
-		UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+		UWARN("Another node %d has already label \"%s\", cannot set it to node %d", idFound, label.c_str(), id);
 	}
 	return false;
 }

corelib/src/Rtabmap.cpp

@@ -873,10 +873,26 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
 		{
 			return _memory->labelSignature(id, label);
 		}
-		else if(_memory->getLastWorkingSignature())
+		else if(_memory->isIncremental() && _memory->getLastWorkingSignature())
 		{
 			return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
 		}
+		else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
+		{
+			std::map<int, Transform> nearestNodes = getNodesInRadius(_lastLocalizationPose, _localRadius, 1);
+			if(!nearestNodes.empty())
+			{
+				return _memory->labelSignature(nearestNodes.begin()->first, label);
+			}
+			else
+			{
+				UERROR("No nodes found inside %s=%fm of the current pose (%s). Cannot set label \"%s\"",
+						Parameters::kRGBDLocalRadius().c_str(),
+						_localRadius,
+						_lastLocalizationPose.prettyPrint().c_str(),
+						label.c_str());
+			}
+		}
 		else
 		{
 			UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
@@ -1729,7 +1745,7 @@ bool Rtabmap::process(
 					if(_optimizedPoses.size() && _memory->isIncremental())
 					{
 						//Search for latest node having GPS linked to current signature not too far.
-						std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+						std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 						for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
 						{
 							const Signature * s = _memory->getSignature(iter->first);
@@ -2225,7 +2241,7 @@ bool Rtabmap::process(
 
 			// retrieval based on the nodes close the the nearest pose in WM
 			// immunize closest nodes
-			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+			std::map<int, float> nearNodes = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 			// sort by distance
 			std::multimap<float, int> nearNodesByDist;
 			for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
@@ -2412,7 +2428,7 @@ bool Rtabmap::process(
 				}
 				else
 				{
-					nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+					nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 				}
 				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
@@ -2463,7 +2479,7 @@ bool Rtabmap::process(
 
 					//find the nearest pose on the path looking in the same direction
 					path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-					path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
+					path = graph::findNearestPoses(signature->id(), path, _localRadius, _proximityAngle);
 					//take the one with highest likelihood if not null
 					int nearestId = 0;
 					if(iter->first.likelihood > 0.0f &&
@@ -4286,7 +4302,7 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		}
 		else
 		{
-			foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
+			foundIds = graph::findNearestNodes(fromId, _optimizedPoses, radius);
 		}
 
 		float radiusSqrd = radius * radius;
@@ -4872,24 +4888,48 @@ void Rtabmap::getGraph(
 	}
 }
 
-std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
+std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius, int k, std::map<int, float> * distsSqr)
 {
-	return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
+	std::map<int, float> nearestNodesTmp;
+	std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
+	*nearestNodesPtr = graph::findNearestNodes(pose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
+	std::map<int, Transform> nearestPoses;
+	for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
+	{
+		nearestPoses.insert(*_optimizedPoses.find(iter->first));
+	}
+	return nearestPoses;
 }
 
-std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
+std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius, int k, std::map<int, float> * distsSqr)
 {
 	UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
-	std::map<int, Transform> nearNodes;
-	if(nodeId==0 && !_optimizedPoses.empty())
+	std::map<int, float> nearestNodesTmp;
+	std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
+	if(nodeId==0 && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
 	{
-		nodeId = _optimizedPoses.rbegin()->first;
+		*nearestNodesPtr = graph::findNearestNodes(_lastLocalizationPose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
 	}
-	if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+	else
 	{
-		nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
+		if(nodeId==0 && !_optimizedPoses.empty())
+		{
+			nodeId = _optimizedPoses.rbegin()->first;
+		}
+
+		if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+		{
+			*nearestNodesPtr = graph::findNearestNodes(nodeId, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
+		}
 	}
-	return nearNodes;
+
+	std::map<int, Transform> nearestPoses;
+	for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
+	{
+		nearestPoses.insert(*_optimizedPoses.find(iter->first));
+	}
+
+	return nearestPoses;
 }
 
 int Rtabmap::detectMoreLoopClosures(

corelib/src/RtabmapThread.cpp

@@ -292,9 +292,16 @@ void RtabmapThread::mainLoop()
 		_rtabmap->clearPath(0);
 		break;
 	case kStateLabelling:
-		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
+		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
 		{
-			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()));
+			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
+		}
+		break;
+	case kStateRemovingLabel:
+		id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
+		if(!_rtabmap->labelLocation(id, ""))
+		{
+			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
 		}
 		break;
 	default: