i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

28
corelib/include/rtabmap/core/Graph.h
View File

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

2
corelib/include/rtabmap/core/Rtabmap.h
View File

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