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

Added initial node to graph computed from A*

Browse Source
This commit is contained in:
Mathieu Labbe
2015-01-26 17:20:43 -05:00
parent f9fb6216d8
commit 47045002a9
2 changed files with 17 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -81,11 +81,26 @@ std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
float angle, float angle,
bool keepLatest = true); bool keepLatest = true);
/**
* Get all neighbor nodes in a fixed radius around each pose.
* @param poses The poses
* @param radius Radius (m) of the search for near neighbors
* @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius
* @return A map between each pose id and its neighbors found in the radius
*/
std::multimap<int, int> RTABMAP_EXP radiusPosesClustering( std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
float radius, float radius,
float angle); float angle);
/**
* Perform A* path planning in the graph.
* @param poses The graph's poses
* @param links The graph's links (from node id -> to node id)
* @param from initial node
* @param to final node
* @return the path ids from id "from" to id "to" including initial and final nodes.
*/
std::vector<int> RTABMAP_EXP computePath( std::vector<int> RTABMAP_EXP computePath(
const std::map<int, rtabmap::Transform> & poses, const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, int> & links, const std::multimap<int, int> & links,

3
corelib/src/Graph.cpp
View File

@@ -719,6 +719,7 @@ std::vector<int> computePath(
path.push_front(currentNode.id()); path.push_front(currentNode.id());
currentNode = nodes.find(currentNode.fromId())->second; currentNode = nodes.find(currentNode.fromId())->second;
} }
path.push_front(startNode);
break; break;
} }
@@ -743,7 +744,7 @@ std::vector<int> computePath(
float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose()); float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose());
if(nodeIter->second.costSoFar() > newCostSoFar) if(nodeIter->second.costSoFar() > newCostSoFar)
{ {
UERROR("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar()); UWARN("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
} }
} }
} }