tf::Pose relativePose (const ConstraintGraph& g, const unsigned n1,
const unsigned n2)
{
const Graph& graph = g.graph();
ShortestPath path = shortestPath(g, n1, n2);
tf::Pose p;
p.setIdentity();
BOOST_FOREACH (const unsigned e, path.second)
p *= util::toPose(graph[g.idEdge(e)].constraint.constraint.pose);
return p;
}
ShortestPath shortestPath (const ConstraintGraph& g,
const unsigned src, const unsigned dest)
{
typedef Graph::out_edge_iterator EdgeIter;
const Graph& graph = g.graph();
// Call Dijkstra's algorithm
const GraphVertex v = g.idVertex(src);
const GraphVertex w = g.idVertex(dest);
const DijkstraResult res = dijkstra(graph, v);
// Check path exists
if (dest!=src && util::keyValue(res.second, w)==w)
throw NoPathException(src, dest);
// Extract node path
list<unsigned> path;
for (GraphVertex x=w; x!=util::keyValue(res.second, x);
x=util::keyValue(res.second, x))
path.push_front(graph[x].id);
path.push_front(src);
ShortestPath ret;
copy(path.begin(), path.end(), back_inserter(ret.first));
// Extract edge path
// \todo Doesn't pick best edge when there are parallel edges
for (unsigned i=0; i<ret.first.size()-1; i++)
{
const unsigned from = ret.first[i];
const unsigned to = ret.first[i+1];
pair<GraphEdge, bool> e = edge(g.idVertex(from), g.idVertex(to), graph);
ROS_ASSERT(e.second);
ret.second.push_back(graph[e.first].id);
}
return ret;
}
