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.

2026-10-06 18:17:47 +08:00 · 2022-01-20 18:56:38 -05:00
parent 3c215b9b4d
commit 83d1e27b81
11 changed files with 262 additions and 177 deletions
@@ -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,