i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-03 16:47:47 +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
corelib/src/Rtabmap.cpp
+20
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,