问题
I have the following undirected graph classes:
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, Vertex, Edge> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;
struct GraphContainer
{
std::map<OcTreeNode*, VertexDesc> revmap;
Graph graph;
};
after populating the graph with vertices and weighted edges, I try to run Dijkstra and extract one path from start to goal.
Note: the graph is a graph of 3D points in space, and the edge weight is given by:
g->graph[e].weight = 1.0 + pow(coord.distance(coord1), 2) + 4 * pow(coord.z() - coord1.z(), 2);
so it is always positive.
Here is the code to get the solution:
GraphContainer *g = ...;
boost::print_graph(g->graph);
octomap::point3d startPos = ...;
VertexDesc start = closestVertexDescriptor(g, startPos);
std::cout << "start: " << start << " " << startPos << std::endl;
octomap::point3d goalPos = ...;
VertexDesc goal = closestVertexDescriptor(g, goalPos);
std::cout << "goal: " << goal << " " << goalPos << std::endl;
auto idmap = boost::get(boost::vertex_index, g->graph);
std::vector<VertexDesc> predecessors(boost::num_vertices(g->graph));
std::vector<int> distances(boost::num_vertices(g->graph));
dijkstra_shortest_paths(g->graph, goal,
boost::weight_map(boost::get(&Edge::weight, g->graph))
.distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
.predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
);
// extract path
std::vector<VertexDesc> path;
VertexDesc current = goal;
while(current != start)
{
std::cout << "extract path: " << current << " " << g->graph[current].coord << " <- " << predecessors[current] << std::endl;
path.push_back(current);
if(current == predecessors[current])
{
std::cout << "extract path: detected cycle!" << std::endl;
break;
}
current = predecessors[current];
}
path.push_back(start);
outputs: (graph printout is too big, get graph.txt at: https://drive.google.com/file/d/1nbTyCo_-tMfpH4bblurv-IYcumjjuKFK/view?usp=sharing)
start: 8088 (0.725003 1.675 1.21072e-05)
goal: 10302 (2.925 4.025 3.77501)
extract path: 10302 (2.95 4.05 3.75) <- 10302
extract path: detected cycle!
with code from https://www.ics.uci.edu/~eppstein/161/python/dijkstra.py plus the following snippet:
G = {}
def add_edge(v0,v1,w=1):
global G
for v in (v0,v1):
if v not in G: G[v] = {}
G[v0][v1] = w
G[v1][v0] = w
with open('graph.txt','rt') as f:
for line in f:
v0, vs = line.rstrip().split(' <--> ')
for v1 in vs.split():
add_edge(v0,v1)
start = '8088'
goal = '10302'
print(' '.join(shortestPath(G, start, goal)))
I verified that a solution exists (using unitary weights, as boost::print_graph does not print weights, but it should not be relevant):
8088 7803 7783 7759 5811 5809 5799 5796 5712 5702 5674 5666 2168 2009 1985 1967 1934 1928 1906 1900 1633 1626 1597 1617 1187 1303 1299 1319 1283 1370 1420 1448 1463 4942 4963 4969 4972 5083 5089 5100 5107 5421 5430 5443 5445 5480 5500 5528 5994 5998 6000 6001 6016 6194 6196 6198 6199 6200 6202 6204 6206 6208 6210 6212 6214 8368 8370 9790 9792 10010 10021 10026 10053 10094 10098 10110 10122 10190 10202 10302
so why with boost the solution is not found and/or the path contains a cycle?
回答1:
dijkstra_shortest_paths initializes the distance and predecessor maps. What you're seeing is likely the effect that a vertex has no predecessor, hence its predecessor is itself. That terminates your loop and (falsely) reports a cycle.
It's easy to reproduce by creating a graph with no edges at all.
Moreover, I noticed you accidentally passed the goal to dijkstra_shortest_paths where you should have passed start:
dijkstra_shortest_paths(gc.graph, start,
boost::weight_map(boost::get(&Edge::weight, gc.graph))
.distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
.predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
);
Now, finally, if you want the weight of the edges to be that distance function, consider using a distance function:
BONUS DEMO
Changes for illustration:
- using Boost Geometry
rtreeinstead ofoctomap(unknown to me). It has nearest point queries and thedistancefunction - Using a dynamic
weight_map()function - Encapsulating
closest(),find_path(),print(), and addingsave_dotto allow visualization - Using "random" generated graph of 10 vertices with random points inside {-5,-5,-5}x{+5,+5,+5}, with 10:50 (1 in 6) possible edges added. The random seed has been fixed to achieve the demo below (uncomment
random_deviceto allow random data)
int main() {
GraphContainer gc = GraphContainer::generate_random();
gc.print();
auto start = gc.closest({1,1,1}),
goal = gc.closest({10,10,10});
auto coordmap = get(&Vertex::coord, gc.graph());
std::cout << "start: " << start << " " << coordmap[start] << "\n";
std::cout << "goal: " << goal << " " << coordmap[goal] << "\n";
PathType const path = gc.find_path(start, goal);
std::cout << "Path: ";
for (auto vd : path)
std::cout << vd << " (" << coordmap[vd] << ") ";
if (path.front() != start)
std::cout << "PATH INCOMPLETE\n";
std::cout << "\n";
gc.save_dot("graph.dot", path);
}
Notice how the code is clean and structured. Simple helper functions are simple:
#include <boost/graph/graph_utility.hpp>
void GraphContainer::print() const {
print_graph(_graph);
}
Finding the nearest vertex:
VertexDesc closest(octomap::point3d const& where) const {
octomap::point3d closest[1];
size_t n = bgi::query(_tree, bgi::nearest(where, 1), closest);
assert(1 == n);
return revmap.at(*closest); // throws if missing revmap entry
}
Finding a path directly from coordinates:
PathType find_path(octomap::point3d const& startPos, octomap::point3d const& goalPos) const {
return find_path(closest(startPos), closest(goalPos));
}
The weights get calculated on the fly using the boost::geometry::distance implementation:
struct Weight {
Graph const& g;
double operator()(EdgeDesc const& ed) const {
auto s = g[source(ed, g)].coord,
t = g[target(ed, g)].coord;
return bg::distance(s, t);
}
};
boost::function_property_map<Weight, EdgeDesc, double> weight_map() const {
return boost::make_function_property_map<EdgeDesc>(Weight{_graph});
}
In the end we save the graph in GraphViz format to be rendered with e.g. dot:
#include <boost/graph/graphviz.hpp>
void GraphContainer::save_dot(std::string const& fname, PathType const& mark) const {
boost::dynamic_properties dp;
auto idmap = boost::get(boost::vertex_index, _graph);
dp.property("node_id", idmap);
dp.property("label", weight_map());
auto emphasis = [&mark](VertexDesc vd) { return std::count(mark.begin(), mark.end(), vd); };
dp.property("color", boost::make_transform_value_property_map([=](VertexDesc vd) {
return emphasis(vd)? "red" : "black";
}, idmap));
dp.property("color", boost::make_function_property_map<EdgeDesc>([=](EdgeDesc ed) {
return emphasis(source(ed, _graph)) && emphasis(target(ed, _graph))? "red" : "black";
}));
std::ofstream ofs(fname);
write_graphviz_dp(ofs, _graph, dp);
}
It marks the nodes/edges involved in the path in red:
Full Listing
Live On Coliru
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
namespace octomap {
struct point3d { double x,y,z; };
static inline std::ostream& operator<<(std::ostream& os, point3d const& pt) {
return os << "{" << pt.x << "," << pt.y << "," << pt.z << "}";
}
}
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/index/rtree.hpp>
namespace bg = boost::geometry;
BOOST_GEOMETRY_REGISTER_POINT_3D(octomap::point3d, double, bg::cs::cartesian, x, y, z)
#include <random>
static std::mt19937 s_prng { 2 }; // { std::random_device{}() };
namespace bgi = bg::index;
namespace MockupTree {
using octomap::point3d;
using Tree = bgi::rtree<point3d, bgi::rstar<32> >;
Tree generate_random() {
std::uniform_real_distribution<double> dist(-5, 5);
auto gen = [&] { return point3d { dist(s_prng), dist(s_prng), dist(s_prng) }; };
Tree tree;
for (auto i = 0; i < 10; ++i)
tree.insert(gen());
return tree;
}
}
using Tree = MockupTree::Tree;
using OcTreeNode = const Tree::value_type;
struct Vertex {
octomap::point3d coord;
OcTreeNode *node;
};
typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, Vertex> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;
using PathType = std::vector<VertexDesc>;
class GraphContainer {
private:
GraphContainer() = default;
public:
static GraphContainer generate_random() {
GraphContainer result;
result._tree = MockupTree::generate_random();
auto& g = result._graph;
// add all the vertices (also in revmap)
for (auto& pt : result._tree) {
VertexDesc vd = add_vertex(Vertex{ pt, nullptr }, g);
auto insertion = result.revmap.emplace(pt, vd);
assert(insertion.second); // no duplicate points please
}
// add random part of edges
std::discrete_distribution<> p({50, 10}); // 1 out of 6
for (auto from : boost::make_iterator_range(vertices(g)))
for (auto to : boost::make_iterator_range(vertices(g))) {
if (from != to && p(s_prng)) {
auto e = boost::edge(from, to, g);
if (!e.second) // edge didn't exist yet
add_edge(from, to, g);
}
}
return result;
}
Graph const& graph() const { return _graph; }
VertexDesc closest(octomap::point3d const& where) const {
octomap::point3d closest[1];
size_t n = bgi::query(_tree, bgi::nearest(where, 1), closest);
assert(1 == n);
return revmap.at(*closest); // throws if missing revmap entry
}
PathType find_path(octomap::point3d const& startPos, octomap::point3d const& goalPos) const {
return find_path(closest(startPos), closest(goalPos));
}
PathType find_path(VertexDesc start, VertexDesc goal) const {
auto idmap = boost::get(boost::vertex_index, _graph);
std::vector<VertexDesc> predecessors(boost::num_vertices(_graph), _graph.null_vertex());
std::vector<int> distances(boost::num_vertices(_graph));
dijkstra_shortest_paths(_graph, start,
boost::weight_map(weight_map())
.distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
.predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
);
// extract path
VertexDesc current = goal;
PathType path { current };
do {
auto const pred = predecessors.at(current);
//std::cout << "extract path: " << current << " " << _graph[current].coord << " <- " << pred << std::endl;
if(current == pred)
break;
current = pred;
path.push_back(current);
} while(current != start);
std::reverse(path.begin(), path.end());
return path;
}
struct Weight {
Graph const& g;
double operator()(EdgeDesc const& ed) const {
auto s = g[source(ed, g)].coord,
t = g[target(ed, g)].coord;
return bg::distance(s, t);
}
};
boost::function_property_map<Weight, EdgeDesc, double> weight_map() const {
return boost::make_function_property_map<EdgeDesc>(Weight{_graph});
}
void print() const;
void save_dot(std::string const& fname, PathType const& emphasize = {}) const;
private:
Tree _tree;
Graph _graph;
//std::map<OcTreeNode*, VertexDesc> revmap;
struct PointCompare {
bool operator()(octomap::point3d const& a, octomap::point3d const& b) const {
return std::tie(a.x, a.y, a.z) < std::tie(b.x, b.y, b.z);
}
};
std::map<octomap::point3d, VertexDesc, PointCompare> revmap;
};
#include <boost/graph/graph_utility.hpp>
void GraphContainer::print() const {
print_graph(_graph);
}
#include <boost/graph/graphviz.hpp>
void GraphContainer::save_dot(std::string const& fname, PathType const& mark) const {
boost::dynamic_properties dp;
auto idmap = boost::get(boost::vertex_index, _graph);
dp.property("node_id", idmap);
dp.property("label", weight_map());
auto emphasis = [&mark](VertexDesc vd) { return std::count(mark.begin(), mark.end(), vd); };
dp.property("color", boost::make_transform_value_property_map([=](VertexDesc vd) {
return emphasis(vd)? "red" : "black";
}, idmap));
dp.property("color", boost::make_function_property_map<EdgeDesc>([=](EdgeDesc ed) {
return emphasis(source(ed, _graph)) && emphasis(target(ed, _graph))? "red" : "black";
}));
std::ofstream ofs(fname);
write_graphviz_dp(ofs, _graph, dp);
}
int main() {
GraphContainer gc = GraphContainer::generate_random();
gc.print();
auto start = gc.closest({1,1,1}),
goal = gc.closest({10,10,10});
auto coordmap = get(&Vertex::coord, gc.graph());
std::cout << "start: " << start << " " << coordmap[start] << "\n";
std::cout << "goal: " << goal << " " << coordmap[goal] << "\n";
PathType const path = gc.find_path(start, goal);
std::cout << "Path: ";
for (auto vd : path)
std::cout << vd << " (" << coordmap[vd] << ") ";
if (path.front() != start)
std::cout << "PATH INCOMPLETE\n";
std::cout << "\n";
gc.save_dot("graph.dot", path);
}
Console Output:
0 <--> 7 1 3 6 9
1 <--> 0 4 3 9
2 <--> 4 7 9
3 <--> 0 1 7
4 <--> 1 2 5 9
5 <--> 4 6 9
6 <--> 5 0 7 9
7 <--> 0 3 6 2
8 <--> 9
9 <--> 4 6 8 0 1 2 5
start: 4 {-0.0143883,0.86797,2.19754}
goal: 3 {1.32738,3.18227,1.83026}
Path: 4 ({-0.0143883,0.86797,2.19754}) 1 ({-0.152509,-1.79464,-3.45573}) 3 ({1.32738,3.18227,1.83026})
Graphviz output in graph.dot:
graph G {
0 [color=black];
1 [color=red];
2 [color=black];
3 [color=red];
4 [color=red];
5 [color=black];
6 [color=black];
7 [color=black];
8 [color=black];
9 [color=black];
0--7 [color=black, label=11.2344];
1--0 [color=black, label=10.4521];
1--4 [color=red, label=6.25045];
2--4 [color=black, label=5.59547];
3--0 [color=black, label=5.32263];
3--1 [color=red, label=7.40954];
3--7 [color=black, label=9.29024];
4--5 [color=black, label=3.96107];
4--9 [color=black, label=6.14037];
5--6 [color=black, label=4.45081];
6--0 [color=black, label=6.51876];
6--7 [color=black, label=8.683];
6--9 [color=black, label=7.2063];
7--2 [color=black, label=5.10893];
8--9 [color=black, label=2.78728];
9--0 [color=black, label=10.3583];
9--1 [color=black, label=3.8217];
9--2 [color=black, label=5.60891];
9--5 [color=black, label=5.63377];
}
来源：https://stackoverflow.com/questions/48367649/boost-dijkstra-shortest-paths-cant-extract-or-find-the-path-path-contains