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

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.

Browse Source
This commit is contained in:
matlabbe
2022-01-20 18:56:38 -05:00
parent 3c215b9b4d
commit 83d1e27b81
11 changed files with 262 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -267,54 +267,58 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
float angularVelocity = 0.0f); // rad/sec
/**
* Get the nearest node of the target pose
* Find 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 std::map<int, rtabmap::Transform> & poses,
const rtabmap::Transform & targetPose,
float * distance = 0);
/**
* 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);
/**
* Get nodes near the query
* Find the nearest nodes of the query pose or node
* @param nodeId the query id
* @param nodes the nodes to search for
* @param radius radius to search for (m)
* @param radius radius to search for (m), if 0, k should be > 0.
* @param k max nearest neighbors (0=all inside the radius)
* @return the nodes with squared distance to query node.
*/
std::map<int, float> RTABMAP_EXP getNodesInRadius(
std::map<int, float> RTABMAP_EXP findNearestNodes(
int nodeId,
const std::map<int, Transform> & nodes,
float radius);
std::map<int, float> RTABMAP_EXP getNodesInRadius(
const std::map<int, Transform> & poses,
float radius,
float angle = 0.0f,
int k=0);
std::map<int, float> RTABMAP_EXP findNearestNodes(
const Transform & targetPose,
const std::map<int, Transform> & nodes,
float radius);
std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
const std::map<int, Transform> & poses,
float radius,
float angle = 0.0f,
int k=0);
std::map<int, Transform> RTABMAP_EXP findNearestPoses(
int nodeId,
const std::map<int, Transform> & nodes,
const std::map<int, Transform> & poses,
float radius,
float angle = 0.0f);
std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
float angle = 0.0f,
int k=0);
std::map<int, Transform> RTABMAP_EXP findNearestPoses(
const Transform & targetPose,
const std::map<int, Transform> & nodes,
const std::map<int, Transform> & poses,
float radius,
float angle = 0.0f);
float angle = 0.0f,
int k=0);
// typedef hack to avoid error with RTABMAP_DEPRECATED
typedef std::map<int, float> _mapIntFloat;
typedef std::map<int, Transform> _mapIntTransform;
RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
float RTABMAP_EXP computePathLength(
const std::vector<std::pair<int, Transform> > & path,

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

@@ -196,8 +196,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.
std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
int detectMoreLoopClosures(
float clusterRadiusMax = 0.5f,
float clusterAngle = M_PI/6.0f,

3
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -73,7 +73,8 @@ public:
kCmdResume,
kCmdGoal, // params: [string] label or [int] location ID
kCmdCancelGoal,
kCmdLabel // params: [string] label, [int] location ID
kCmdLabel, // params: [string] label, [int] location ID
kCmdRemoveLabel // params: [string] label
};
public:
RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :

3
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -68,7 +68,8 @@ public:
kStateTriggeringMap,
kStateSettingGoal,
kStateCancellingGoal,
kStateLabelling
kStateLabelling,
kStateRemovingLabel
};
public: