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/include/rtabmap/core/Graph.h

@@ -267,54 +267,58 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float angularVelocity = 0.0f); // rad/sec
 
 /**
- * Get the nearest node of the target pose
+ * Find the nearest node of the target pose
  * @param nodes the nodes to search for
  * @param targetPose the target pose to search around
  * @param distance squared distance of the nearest node found (optional)
  * @return the node id.
  */
 int RTABMAP_EXP findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance = 0);
 
 /**
- * Get K nearest nodes of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param k number of nearest neighbors to search for
- * @return the node ids with squared distance to target pose.
- */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		int k);
-
-/**
- * Get nodes near the query
+ * Find the nearest nodes of the query pose or node
  * @param nodeId the query id
  * @param nodes the nodes to search for
- * @param radius radius to search for (m)
+ * @param radius radius to search for (m), if 0, k should be > 0.
+ * @param k max nearest neighbors (0=all inside the radius)
  * @return the nodes with squared distance to query node.
  */
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
+		float angle = 0.0f,
+		int k=0);
+
+// typedef hack to avoid error with RTABMAP_DEPRECATED
+typedef std::map<int, float> _mapIntFloat;
+typedef std::map<int, Transform> _mapIntTransform;
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
 
 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,

corelib/include/rtabmap/core/Rtabmap.h

@@ -196,8 +196,8 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -73,7 +73,8 @@ public:
 			kCmdResume,
 			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel             // params: [string] label, [int] location ID
+			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdRemoveLabel       // params: [string] label
 	};
 public:
 	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :

corelib/include/rtabmap/core/RtabmapThread.h

@@ -68,7 +68,8 @@ public:
 		kStateTriggeringMap,
 		kStateSettingGoal,
 		kStateCancellingGoal,
-		kStateLabelling
+		kStateLabelling,
+		kStateRemovingLabel
 	};
 
 public:

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:

guilib/include/rtabmap/gui/MainWindow.h

@@ -194,6 +194,7 @@ protected Q_SLOTS:
 	void postGoal(const QString & goal);
 	void cancelGoal();
 	void label();
+	void removeLabel();
 	void updateCacheFromDatabase();
 	void anchorCloudsToGroundTruth();
 	void selectScreenCaptureFormat(bool checked);

guilib/src/MainWindow.cpp

@@ -354,6 +354,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
 	connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
 	connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
+	connect(_ui->actionRemove_label, SIGNAL(triggered()), this, SLOT(removeLabel()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
@@ -2300,7 +2301,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			// update pose only if odometry is not received
 			std::map<int, int> mapIds = _currentMapIds;
 			std::map<int, Transform> groundTruth = _currentGTPosesMap;
-			std::map<int, std::string> labels = _currentLabels;
 
 			mapIds.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().mapId()));
 			if(!stat.getLastSignatureData().getGroundTruthPose().isNull() &&
@@ -2308,10 +2308,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				groundTruth.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getGroundTruthPose()));
 			}
-			for(std::map<int, std::string>::const_iterator iter=stat.labels().begin(); iter!=stat.labels().end(); ++iter)
-			{
-				uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified
-			}
 
 			if(_preferencesDialog->isPriorIgnored() &&
 				_ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
@@ -2413,7 +2409,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					poses,
 					stat.constraints(),
 					mapIds,
-					labels,
+					stat.labels(),
 					groundTruth,
 					stat.odomCachePoses(),
 					stat.odomCacheConstraints(),
@@ -2646,7 +2642,7 @@ void MainWindow::updateMapCloud(
 		std::map<int, Transform> nearestPoses;
 		if(maxNodes > 0)
 		{
-			std::map<int, float> nodes = graph::findNearestNodes(poses, currentPose, maxNodes);
+			std::map<int, float> nodes = graph::findNearestNodes(currentPose, poses, 0, 0, maxNodes);
 			for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 			{
 				if(altitudeDelta<=0.0 ||
@@ -6947,6 +6943,17 @@ void MainWindow::label()
 	}
 }
 
+void MainWindow::removeLabel()
+{
+	UINFO("Removing label...");
+	bool ok = false;
+	QString label = QInputDialog::getText(this, tr("Remove label"), tr("Label: "), QLineEdit::Normal, "", &ok);
+	if(ok && !label.isEmpty())
+	{
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRemoveLabel, label.toStdString(), 0));
+	}
+}
+
 void MainWindow::updateCacheFromDatabase()
 {
 	QString dir = getWorkingDirectory();

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>21</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -351,6 +351,7 @@
     <addaction name="actionTrigger_a_new_map"/>
     <addaction name="separator"/>
     <addaction name="actionLabel_current_location"/>
+    <addaction name="actionRemove_label"/>
     <addaction name="actionSend_waypoints"/>
     <addaction name="actionSend_goal"/>
     <addaction name="actionCancel_goal"/>
@@ -1670,6 +1671,11 @@
     <string>OpenNI2</string>
    </property>
   </action>
+  <action name="actionRemove_label">
+   <property name="text">
+    <string>Remove label...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>