boost dijkstra_shortest_paths：无法提取（或找到？）路径（路径包含一个循环）
我有以下无向图表类：
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并从开始到目标提取一条路径。
注意：图表是空间中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);
输出:(图形打印输出太大，请在以下位置获取graph.txt：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!
使用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 5696 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
那么为什么没有找到提升解决方案和/或路径包含一个循环？
1 个答案:
答案 0 :(得分：1)
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))
);
现在，最后，如果您希望边缘的权重为距离函数，请考虑使用距离函数：
BONUS DEMO
插图的更改：
- 使用Boost Geometry
rtree代替octomap（我不知道）。它有最近的点查询和distance函数 - 使用动态
weight_map()功能 - 封装
closest()，find_path()，print()，并添加save_dot以允许可视化 - 使用“随机”生成的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中路径中涉及的节点/边：
完整列表
<强> 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);
}
控制台输出：
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];
}
相关问题
最新问题
- 我写了这段代码，但我无法理解我的错误
- 我无法从一个代码实例的列表中删除 None 值，但我可以在另一个实例中。为什么它适用于一个细分市场而不适用于另一个细分市场？
- 是否有可能使 loadstring 不可能等于打印？卢阿
- java中的random.expovariate()
- Appscript 通过会议在 Google 日历中发送电子邮件和创建活动
- 为什么我的 Onclick 箭头功能在 React 中不起作用？
- 在此代码中是否有使用“this”的替代方法？
- 在 SQL Server 和 PostgreSQL 上查询，我如何从第一个表获得第二个表的可视化
- 每千个数字得到
- 更新了城市边界 KML 文件的来源？