i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rtabmap: added getNodesInRadius() public functions

Browse Source
This commit is contained in:
matlabbe
2020-09-28 12:03:42 -04:00
parent 933ac736f1
commit bedc771fa4
5 changed files with 102 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
corelib/src/Graph.cpp
View File

@@ -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);
if(nearestNodes.size())
std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
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());
ids[oi] = iter->first;
++oi;
}
(*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);
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());
ids[oi] = iter->first;
++oi;
}
(*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);
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)
{

20
corelib/src/Rtabmap.cpp
View File

@@ -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,