Rtabmap: added getNodesInRadius() public functions

corelib/include/rtabmap/core/Graph.h

@@ -255,11 +255,26 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float linearVelocity = 0.0f,   // m/sec
 		float angularVelocity = 0.0f); // rad/sec
 
+/**
+ * Get 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 rtabmap::Transform & targetPose);
+		const rtabmap::Transform & targetPose,
+		float * distance = 0);
 
-std::vector<int> RTABMAP_EXP findNearestNodes(
+/**
+ * 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);
@@ -275,11 +290,20 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius);
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius);
 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius,
 		float angle = 0.0f);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle = 0.0f);
 
 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,

corelib/include/rtabmap/core/Rtabmap.h

@@ -198,6 +198,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.
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,

corelib/src/Graph.cpp

@@ -2054,23 +2054,28 @@ std::list<std::pair<int, Transform> > computePath(
 
 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose)
+		const rtabmap::Transform & targetPose,
+		float * distance)
 {
 	int id = 0;
-	std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
-	if(nearestNodes.size())
+	if(!nearestNodes.empty())
 	{
-		id = nearestNodes[0];
+		id = nearestNodes.begin()->first;
+		if(distance)
+		{
+			*distance = nearestNodes.begin()->second;
+		}
 	}
 	return id;
 }
 
-std::vector<int> findNearestNodes(
+std::map<int, float> findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k)
 {
-	std::vector<int> nearestIds;
+	std::map<int, float> nearestIds;
 	if(nodes.size() && !targetPose.isNull())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2090,10 +2095,9 @@ std::vector<int> findNearestNodes(
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->nearestKSearch(pt, k, ind, dist);
 
-		nearestIds.resize(ind.size());
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			nearestIds[i] = ids[ind[i]];
+			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
 		}
 	}
 	return nearestIds;
@@ -2106,8 +2110,21 @@ std::map<int, float> getNodesInRadius(
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = nodes;
+	Transform targetPose = nodes.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+}
+
+// return <id, sqrd distance>, excluding query
+std::map<int, float> getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius)
+{
 	std::map<int, float> foundNodes;
-	if(nodes.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2118,26 +2135,21 @@ std::map<int, float> getNodesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
+		(*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());
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		ids[oi] = iter->first;
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		++oi;
-			ids[oi] = iter->first;
-			++oi;
-		}
 	}
 	cloud->resize(oi);
 	ids.resize(oi);
 
-	Transform fromT = nodes.at(nodeId);
-
 	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(fromT.x(), fromT.y(), fromT.z());
+		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)
 		{
@@ -2159,8 +2171,21 @@ std::map<int, Transform> getPosesInRadius(
 		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.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2171,29 +2196,24 @@ std::map<int, Transform> getPosesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
+		(*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());
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		ids[oi] = iter->first;
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		++oi;
-			ids[oi] = iter->first;
-			++oi;
-		}
 	}
 	cloud->resize(oi);
 	ids.resize(oi);
 
-	Transform fromT = nodes.at(nodeId);
-
 	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(fromT.x(), fromT.y(), fromT.z());
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 
-		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{

corelib/src/Rtabmap.cpp

@@ -4568,6 +4568,26 @@ void Rtabmap::getGraph(
 	}
 }
 
+std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
+{
+	return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
+}
+
+std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
+{
+	UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
+	std::map<int, Transform> nearNodes;
+	if(nodeId==0 && !_optimizedPoses.empty())
+	{
+		nodeId = _optimizedPoses.rbegin()->first;
+	}
+	if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+	{
+		nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
+	}
+	return nearNodes;
+}
+
 int Rtabmap::detectMoreLoopClosures(
 		float clusterRadius,
 		float clusterAngle,

guilib/src/MainWindow.cpp

@@ -2318,12 +2318,12 @@ void MainWindow::updateMapCloud(
 	int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
 	if(maxNodes > 0 && poses.size()>1)
 	{
-		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
+		std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
 		std::map<int, Transform> nearestPoses;
 		nearestPoses.insert(*poses.rbegin());
-		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 		{
-			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			std::map<int, Transform>::iterator pter = poses.find(iter->first);
 			if(pter != poses.end())
 			{
 				nearestPoses.insert(*pter);