boost dijkstra_shortest_paths: 无法提取（或找到？）路径（路径包含循环）
boost dijkstra_shortest_paths: can't extract (or find?) the path (path contains a cycle)
我有以下无向图类：
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;
};
用顶点和加权边填充图后，我尝试 运行 Dijkstra 并提取一条从开始到目标的路径。
注：该图是space中的3D点的图，边权重由下式给出：
g->graph[e].weight = 1.0 + pow(coord.distance(coord1), 2) + 4 * pow(coord.z() - coord1.z(), 2);
所以它总是正数。
这里是获取解决方案的代码：
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);
输出：（图形打印输出太大，在 https://drive.google.com/file/d/1nbTyCo_-tMfpH4bblurv-IYcumjjuKFK/view?usp=sharing 处获取 graph.txt）
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!
使用 https://www.ics.uci.edu/~eppstein/161/python/dijkstra.py 中的代码加上以下代码段：
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)))
我验证了解决方案存在（使用单一权重，因为 boost::print_graph 不打印权重，但它不应该是相关的）：
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
那么为什么使用 boost 找不到解决方案and/or路径包含循环？
dijkstra_shortest_paths 初始化距离和前导图。您所看到的可能是顶点没有前驱的效果，因此它的前驱就是它本身。这会终止您的循环并（错误地）报告一个循环。
创建一个完全没有边的图很容易重现。
此外，我注意到您不小心将 goal 传递给了 dijkstra_shortest_paths，而您本应传递 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))
);
现在，最后，如果您希望边缘的权重成为该距离函数，请考虑使用距离函数：
奖金演示
插图更改：
- 使用 Boost Geometry
rtree 而不是 octomap（我不知道）。它有最近点查询和distance函数
- 使用动态
weight_map() 函数
- 封装
closest()、find_path()、print()，并添加save_dot允许可视化
- 使用 "random" 生成的 10 个顶点的图，随机点在 {-5,-5,-5}x{+5,+5,+5} 内，10:50 (1在 6) 中添加了可能的边。已修复随机种子以实现下面的演示（取消注释
random_device 以允许随机数据）
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);
}
请注意代码的整洁和结构。简单的辅助函数很简单：
#include <boost/graph/graph_utility.hpp>
void GraphContainer::print() const {
print_graph(_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));
}
使用 boost::geometry::distance 实现即时计算权重：
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});
}
最后，我们将图形保存为 GraphViz 格式，以便使用例如渲染。 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);
}
表示red中路径涉及的nodes/edges:
完整列表
#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);
}
控制台输出：
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})
graph.dot 中的 Graphviz 输出：
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];
}
我有以下无向图类：
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;
};
用顶点和加权边填充图后，我尝试 运行 Dijkstra 并提取一条从开始到目标的路径。
注：该图是space中的3D点的图，边权重由下式给出：
g->graph[e].weight = 1.0 + pow(coord.distance(coord1), 2) + 4 * pow(coord.z() - coord1.z(), 2);
所以它总是正数。
这里是获取解决方案的代码：
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);
输出：（图形打印输出太大，在 https://drive.google.com/file/d/1nbTyCo_-tMfpH4bblurv-IYcumjjuKFK/view?usp=sharing 处获取 graph.txt）
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!
使用 https://www.ics.uci.edu/~eppstein/161/python/dijkstra.py 中的代码加上以下代码段：
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)))
我验证了解决方案存在（使用单一权重，因为 boost::print_graph 不打印权重，但它不应该是相关的）：
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
那么为什么使用 boost 找不到解决方案and/or路径包含循环？
dijkstra_shortest_paths 初始化距离和前导图。您所看到的可能是顶点没有前驱的效果，因此它的前驱就是它本身。这会终止您的循环并（错误地）报告一个循环。
创建一个完全没有边的图很容易重现。
此外，我注意到您不小心将 goal 传递给了 dijkstra_shortest_paths，而您本应传递 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))
);
现在，最后，如果您希望边缘的权重成为该距离函数，请考虑使用距离函数：
奖金演示
插图更改：
- 使用 Boost Geometry
rtree而不是octomap（我不知道）。它有最近点查询和distance函数 - 使用动态
weight_map()函数 - 封装
closest()、find_path()、print()，并添加save_dot允许可视化 - 使用 "random" 生成的 10 个顶点的图，随机点在 {-5,-5,-5}x{+5,+5,+5} 内，10:50 (1在 6) 中添加了可能的边。已修复随机种子以实现下面的演示（取消注释
random_device以允许随机数据）
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);
}
请注意代码的整洁和结构。简单的辅助函数很简单：
#include <boost/graph/graph_utility.hpp>
void GraphContainer::print() const {
print_graph(_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));
}
使用 boost::geometry::distance 实现即时计算权重：
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});
}
最后，我们将图形保存为 GraphViz 格式，以便使用例如渲染。 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);
}
表示red中路径涉及的nodes/edges:
完整列表
#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);
}
控制台输出：
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})
graph.dot 中的 Graphviz 输出：
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];
}