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

Added rtabmap::computePath() using a pose goal instead of node Id. Graph methods moved in rtabmap::graph namespace

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 13:52:02 -05:00
parent 3d1eccbce1
commit e3074d304c
6 changed files with 226 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
namespace graph {
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
@@ -107,5 +109,17 @@ std::vector<int> RTABMAP_EXP computePath(
int from,
int to);
int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose);
std::map<int, float> RTABMAP_EXP getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
int maxNearestNeighbors,
float radius);
} /* namespace graph */
} /* namespace rtabmap */
#endif /* GRAPH_H_ */

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

@@ -118,11 +118,13 @@ public:
bool optimized,
bool global);
void clearPath();
std::list<std::pair<int, Transform> > computePath(int targetNode);
std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
const std::vector<int> & getPath() const {return _path;}
std::list<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;
int getPathGoalId() const;
int getPathCurrentGoalId() const;
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
@@ -136,6 +138,7 @@ private:
std::map<int, Transform> & optimizedPoses,
std::multimap<int, Link> * constraints = 0) const;
void updateGoalIndex();
bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
@@ -209,6 +212,7 @@ private:
std::vector<int> _path;
unsigned int _pathCurrentIndex;
unsigned int _pathGoalIndex;
Transform _pathTransformToGoal;
};