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

Using Dijkstra for global planning for a significative performance boost (no need to optimize the graph before computing the path)

Browse Source
This commit is contained in:
matlabbe
2015-06-21 18:29:12 -04:00
parent bef408d4b9
commit a5efee20bc
11 changed files with 441 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -101,6 +101,7 @@ public:
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
@@ -137,6 +138,7 @@ private:
virtual void getNodeDataQuery(int signatureId, SensorData & data) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;

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

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
namespace rtabmap {
class Memory;
namespace graph {
@@ -198,6 +199,22 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
int to,
bool updateNewCosts = false);
/**
* Perform Dijkstra path planning in the graph.
* @param fromId initial node
* @param toId final node
* @param memory The graph's memory
* @param lookInDatabase check links in database
* @param updateNewCosts Keep up-to-date costs while traversing the graph.
* @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
*/
std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
int fromId,
int toId,
const Memory * memory,
bool lookInDatabase = true,
bool updateNewCosts = false);
int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose);

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

@@ -115,6 +115,9 @@ public:
bool lookInDatabase = false) const;
std::map<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLinks(int signatureId,
bool lookInDatabase = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
bool isRawDataKept() const {return _rawDataKept;}
bool isBinDataKept() const {return _binDataKept;}
float getSimilarityThreshold() const {return _similarityThreshold;}

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

@@ -291,7 +291,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");

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

@@ -129,7 +129,7 @@ public:
std::map<int, Signature> * signatures = 0);
void clearPath();
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, bool global);
bool computePath(const Transform & targetPose); // only in current optimized map
const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
std::vector<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;